Twix panel to save automatic calibration values to the repo

crates/control/src/calibration_controller.rs

@@ -50,8 +50,7 @@ pub struct CycleContext {
     max_retries_per_capture: Parameter<u32, "calibration_controller.max_retries_per_capture">,
 
     calibration_measurements: AdditionalOutput<Vec<Measurement>, "calibration_inner.measurements">,
-    last_calibration_corrections:
-        AdditionalOutput<Option<Corrections>, "last_calibration_corrections">,
+    last_calibration_corrections: AdditionalOutput<Corrections, "last_calibration_corrections">,
 }
 
 #[context]
@@ -186,9 +185,16 @@ impl CalibrationController {
         context
             .calibration_measurements
             .fill_if_subscribed(|| self.inner_states.measurements.clone());
+
         context
             .last_calibration_corrections
-            .fill_if_subscribed(|| self.corrections);
+            .mutate_if_subscribed(|data| {
+                if let Some(corrections) = self.corrections {
+                    data.replace(corrections);
+                } else {
+                    data.take();
+                }
+            });
 
         Ok(MainOutputs {
             calibration_command: self

crates/control/src/camera_matrix_calculator.rs

@@ -1,15 +1,15 @@
 use std::f32::consts::FRAC_PI_2;
 
 use color_eyre::Result;
-use nalgebra::UnitQuaternion;
+use nalgebra::{UnitQuaternion, Vector3 as NalVec3};
 use projection::{camera_matrices::CameraMatrices, camera_matrix::CameraMatrix, Projection};
 use serde::{Deserialize, Serialize};
 
 use context_attribute::context;
 use coordinate_systems::{Camera, Ground, Head, Pixel, Robot};
 use framework::{AdditionalOutput, MainOutput};
 use geometry::line_segment::LineSegment;
-use linear_algebra::{point, vector, IntoTransform, Isometry3, Vector3};
+use linear_algebra::{point, vector, IntoTransform, Isometry3, Rotation3, Vector3};
 use types::{
     field_dimensions::FieldDimensions, field_lines::ProjectedFieldLines,
     parameters::CameraMatrixParameters, robot_dimensions::RobotDimensions,
@@ -34,6 +34,7 @@ pub struct CycleContext {
     field_dimensions: Parameter<FieldDimensions, "field_dimensions">,
     top_camera_matrix_parameters:
         Parameter<CameraMatrixParameters, "camera_matrix_parameters.vision_top">,
+    robot_rotation_parameters: Parameter<NalVec3<f32>, "camera_matrix_parameters.robot_rotation">,
 }
 
 #[context]
@@ -64,6 +65,14 @@ impl CameraMatrixCalculator {
             context.robot_to_ground.inverse(),
             context.robot_kinematics.head.head_to_robot.inverse(),
             head_to_top_camera,
+        )
+        .to_corrected(
+            Rotation3::from_euler_angles(
+                context.robot_rotation_parameters.x,
+                context.robot_rotation_parameters.y,
+                context.robot_rotation_parameters.z,
+            ),
+            Rotation3::default(),
         );
 
         let head_to_bottom_camera = head_to_camera(
@@ -81,6 +90,14 @@ impl CameraMatrixCalculator {
             context.robot_to_ground.inverse(),
             context.robot_kinematics.head.head_to_robot.inverse(),
             head_to_bottom_camera,
+        )
+        .to_corrected(
+            Rotation3::from_euler_angles(
+                context.robot_rotation_parameters.x,
+                context.robot_rotation_parameters.y,
+                context.robot_rotation_parameters.z,
+            ),
+            Rotation3::default(),
         );
 
         let field_dimensions = context.field_dimensions;

etc/parameters/default.json

@@ -78,6 +78,7 @@
     }
   },
   "camera_matrix_parameters": {
+    "robot_rotation": [0.0, 0.0, 0.0],
     "vision_top": {
       "camera_pitch": -1.2,
       "extrinsic_rotations": [0, 0, 0],

tools/twix/Cargo.toml

@@ -11,6 +11,7 @@ argument_parsers = { workspace = true }
 ball_filter = { workspace = true }
 bincode = { workspace = true }
 buffered_watch = { workspace = true }
+calibration = { workspace = true }
 chrono = { workspace = true }
 clap = { workspace = true }
 color-eyre = { workspace = true }

tools/twix/src/main.rs

@@ -28,9 +28,10 @@ use log::error;
 use nao::Nao;
 use panel::Panel;
 use panels::{
-    BallCandidatePanel, BehaviorSimulatorPanel, EnumPlotPanel, ImageColorSelectPanel, ImagePanel,
-    ImageSegmentsPanel, LookAtPanel, ManualCalibrationPanel, MapPanel, ParameterPanel, PlotPanel,
-    RemotePanel, TextPanel, VisionTunerPanel,
+    AutomaticCameraCalibrationExportPanel, BallCandidatePanel, BehaviorSimulatorPanel,
+    EnumPlotPanel, ImageColorSelectPanel, ImagePanel, ImageSegmentsPanel, LookAtPanel,
+    ManualCalibrationPanel, MapPanel, ParameterPanel, PlotPanel, RemotePanel, TextPanel,
+    VisionTunerPanel,
 };
 
 use reachable_naos::ReachableNaos;
@@ -114,6 +115,7 @@ impl_selectable_panel!(
     ImageSegmentsPanel,
     LookAtPanel,
     ManualCalibrationPanel,
+    AutomaticCameraCalibrationExportPanel,
     MapPanel,
     ParameterPanel,
     PlotPanel,

tools/twix/src/panels/automatic_camera_calibration_export.rs

@@ -0,0 +1,128 @@
+use std::sync::Arc;
+
+use eframe::egui::{Response, Ui, Widget};
+use log::error;
+use nalgebra::Vector3;
+use serde_json::Value;
+
+use calibration::corrections::Corrections;
+use communication::messages::TextOrBinary;
+use parameters::directory::Scope;
+
+use crate::{log_error::LogError, nao::Nao, panel::Panel, value_buffer::BufferHandle};
+
+pub const TOP_CAMERA_EXTRINSICS_PATH: &str =
+    "camera_matrix_parameters.vision_top.extrinsic_rotations";
+pub const BOTTOM_CAMERA_EXTRINSICS_PATH: &str =
+    "camera_matrix_parameters.vision_bottom.extrinsic_rotations";
+pub struct AutomaticCameraCalibrationExportPanel {
+    nao: Arc<Nao>,
+    top_camera: BufferHandle<Vector3<f32>>,
+    bottom_camera: BufferHandle<Vector3<f32>>,
+    calibration_corrections: BufferHandle<Value>,
+}
+
+impl Panel for AutomaticCameraCalibrationExportPanel {
+    const NAME: &'static str = "Automatic Calibration";
+
+    fn new(nao: Arc<Nao>, _value: Option<&Value>) -> Self {
+        let top_camera = nao.subscribe_value(format!("parameters.{TOP_CAMERA_EXTRINSICS_PATH}"));
+        let bottom_camera =
+            nao.subscribe_value(format!("parameters.{BOTTOM_CAMERA_EXTRINSICS_PATH}"));
+        let calibration_corrections =
+            nao.subscribe_json("Control.additional_outputs.last_calibration_corrections");
+
+        Self {
+            nao,
+            top_camera,
+            bottom_camera,
+            calibration_corrections,
+        }
+    }
+}
+
+impl Widget for &mut AutomaticCameraCalibrationExportPanel {
+    fn ui(self, ui: &mut Ui) -> Response {
+        ui.style_mut().spacing.slider_width = ui.available_size().x - 250.0;
+        ui.vertical(|ui| {
+            if let Some(value) = self
+                .calibration_corrections
+                .get_last_value()
+                .ok()
+                .flatten()
+                .and_then(|value| serde_json::from_value::<Corrections>(value).ok())
+            {
+                let top_angles = value.correction_in_camera_top.clone().euler_angles();
+                let bottom_angles = value.correction_in_camera_bottom.euler_angles();
+                let body_angles = value.correction_in_robot.euler_angles();
+
+                draw_group(ui, "Top", top_angles, &self.nao, TOP_CAMERA_EXTRINSICS_PATH);
+                draw_angles_from_buffer(ui, &self.top_camera);
+                ui.separator();
+
+                draw_group(
+                    ui,
+                    "Bottom",
+                    bottom_angles,
+                    &self.nao,
+                    BOTTOM_CAMERA_EXTRINSICS_PATH,
+                );
+                draw_angles_from_buffer(ui, &self.bottom_camera);
+                ui.separator();
+
+                draw_group(
+                    ui,
+                    "Body",
+                    body_angles,
+                    &self.nao,
+                    "camera_matrix_parameters.robot_rotation",
+                );
+                draw_angles(ui, body_angles, "Calibrated");
+            } else {
+                ui.label("Not yet calibrated");
+            }
+        })
+        .response
+    }
+}
+
+fn serialize_and_call<V: serde::Serialize, T: FnOnce(serde_json::Value)>(data: V, callback: T) {
+    match serde_json::to_value(data) {
+        Ok(value) => {
+            callback(value);
+        }
+        Err(error) => error!("failed to serialize parameter value: {error:#?}"),
+    }
+}
+
+fn draw_group(ui: &mut Ui, label: &str, rotations: (f32, f32, f32), nao: &Nao, path: &str) {
+    ui.horizontal(|ui| {
+        ui.label(label);
+        if ui.button("Save to repo").clicked() {
+            serialize_and_call(rotations, |value| {
+                nao.store_parameters(path, value, Scope::default_head())
+                    .log_err();
+            });
+        }
+        if ui.button("Set in Nao").clicked() {
+            serialize_and_call(rotations, |value| {
+                nao.write(format!("parameters.{path}"), TextOrBinary::Text(value));
+            });
+        }
+    });
+    draw_angles(ui, rotations, "Calibrated");
+}
+
+fn draw_angles_from_buffer(ui: &mut Ui, current_values: &BufferHandle<Vector3<f32>>) {
+    if let Some(value) = current_values.get_last_value().ok().flatten() {
+        draw_angles(ui, (value.x, value.y, value.z), "Current");
+    }
+}
+fn draw_angles(ui: &mut Ui, rotations: (f32, f32, f32), sublabel: &str) {
+    ui.label(format!(
+        "{sublabel}: [{0:.2}°, {1:.2}°, {2:.2}°]",
+        rotations.0.to_degrees(),
+        rotations.1.to_degrees(),
+        rotations.2.to_degrees()
+    ));
+}

tools/twix/src/panels/manual_camera_calibration.rs

@@ -7,7 +7,13 @@ use nalgebra::Vector3;
 use parameters::directory::Scope;
 use serde_json::Value;
 
-use crate::{log_error::LogError, nao::Nao, panel::Panel, value_buffer::BufferHandle};
+use crate::{
+    log_error::LogError,
+    nao::Nao,
+    panel::Panel,
+    panels::{BOTTOM_CAMERA_EXTRINSICS_PATH, TOP_CAMERA_EXTRINSICS_PATH},
+    value_buffer::BufferHandle,
+};
 
 pub struct ManualCalibrationPanel {
     nao: Arc<Nao>,
@@ -19,12 +25,9 @@ impl Panel for ManualCalibrationPanel {
     const NAME: &'static str = "Manual Calibration";
 
     fn new(nao: Arc<Nao>, _value: Option<&Value>) -> Self {
-        let top_camera = nao.subscribe_value(
-            "parameters.camera_matrix_parameters.vision_top.extrinsic_rotations".to_string(),
-        );
-        let bottom_camera = nao.subscribe_value(
-            "parameters.camera_matrix_parameters.vision_bottom.extrinsic_rotations".to_string(),
-        );
+        let top_camera = nao.subscribe_value(format!("parameters.{TOP_CAMERA_EXTRINSICS_PATH}"));
+        let bottom_camera =
+            nao.subscribe_value(format!("parameters.{BOTTOM_CAMERA_EXTRINSICS_PATH}"));
 
         Self {
             nao,
@@ -44,7 +47,7 @@ impl Widget for &mut ManualCalibrationPanel {
                     "Top Camera",
                     value,
                     &self.nao,
-                    "camera_matrix_parameters.vision_top.extrinsic_rotations",
+                    TOP_CAMERA_EXTRINSICS_PATH,
                 );
             }
             ui.separator();
@@ -54,7 +57,7 @@ impl Widget for &mut ManualCalibrationPanel {
                     "Bottom Camera",
                     value,
                     &self.nao,
-                    "camera_matrix_parameters.vision_bottom.extrinsic_rotations",
+                    BOTTOM_CAMERA_EXTRINSICS_PATH,
                 );
             }
         })
@@ -82,36 +85,43 @@ fn draw_calibration_ui(
             }
         }
     });
+
+    // DO NOT REMOVE THIS.
+    // In order to save user's sanity, roll, pitch, yaw are swapped to the actual way an airplane fly
+    // rotations.{x,y,z} are in OpenCV convention (z to robot front)
+    // roll -> z
+    // pitch -> x
+    // yaw -> y
     let range = -15.0..=15.0;
-    let mut roll = rotations.x;
+    let mut roll = rotations.z; // See above
     let response = ui.add(
         Slider::new(&mut roll, range.clone())
             .text("Roll")
             .smart_aim(false),
     );
     if response.changed() {
         nao.write(
-            format!("parameters.{path}.x"),
+            format!("parameters.{path}.z"),
             TextOrBinary::Text(serde_json::to_value(roll).unwrap()),
         );
     }
-    let mut pitch = rotations.y;
+    let mut pitch = rotations.x; // See above
     let response = ui.add(
         Slider::new(&mut pitch, range.clone())
             .text("Pitch")
             .smart_aim(false),
     );
     if response.changed() {
         nao.write(
-            format!("parameters.{path}.y"),
+            format!("parameters.{path}.x"),
             TextOrBinary::Text(serde_json::to_value(pitch).unwrap()),
         );
     }
-    let mut yaw = rotations.z;
+    let mut yaw = rotations.y; // See above
     let response = ui.add(Slider::new(&mut yaw, range).text("Yaw").smart_aim(false));
     if response.changed() {
         nao.write(
-            format!("parameters.{path}.z"),
+            format!("parameters.{path}.y"),
             TextOrBinary::Text(serde_json::to_value(yaw).unwrap()),
         );
     }

tools/twix/src/panels/mod.rs

@@ -1,3 +1,4 @@
+mod automatic_camera_calibration_export;
 mod ball_candidates;
 mod behavior_simulator;
 mod enum_plot;
@@ -13,6 +14,10 @@ mod remote;
 mod text;
 mod vision_tuner;
 
+pub use automatic_camera_calibration_export::{
+    AutomaticCameraCalibrationExportPanel, BOTTOM_CAMERA_EXTRINSICS_PATH,
+    TOP_CAMERA_EXTRINSICS_PATH,
+};
 pub use ball_candidates::BallCandidatePanel;
 pub use behavior_simulator::BehaviorSimulatorPanel;
 pub use enum_plot::EnumPlotPanel;