diff --git a/Cargo.lock b/Cargo.lock index 27832ac8e9..d3d16ed362 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -7091,6 +7091,7 @@ dependencies = [ "ball_filter", "bincode", "buffered_watch", + "calibration", "chrono", "clap 4.5.26", "color-eyre", diff --git a/crates/control/src/calibration_controller.rs b/crates/control/src/calibration_controller.rs index f45a328c38..0e29d8f12b 100644 --- a/crates/control/src/calibration_controller.rs +++ b/crates/control/src/calibration_controller.rs @@ -50,8 +50,7 @@ pub struct CycleContext { max_retries_per_capture: Parameter, calibration_measurements: AdditionalOutput, "calibration_inner.measurements">, - last_calibration_corrections: - AdditionalOutput, "last_calibration_corrections">, + last_calibration_corrections: AdditionalOutput, } #[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 diff --git a/crates/control/src/camera_matrix_calculator.rs b/crates/control/src/camera_matrix_calculator.rs index e514f4a61d..f6adce11ea 100644 --- a/crates/control/src/camera_matrix_calculator.rs +++ b/crates/control/src/camera_matrix_calculator.rs @@ -1,7 +1,7 @@ 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}; @@ -9,7 +9,7 @@ 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, top_camera_matrix_parameters: Parameter, + robot_rotation_parameters: Parameter, "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; diff --git a/etc/parameters/default.json b/etc/parameters/default.json index f8c8a981e8..a7e726ea8c 100644 --- a/etc/parameters/default.json +++ b/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], diff --git a/tools/twix/Cargo.toml b/tools/twix/Cargo.toml index 086f381b14..73fa9ef91a 100644 --- a/tools/twix/Cargo.toml +++ b/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 } diff --git a/tools/twix/src/main.rs b/tools/twix/src/main.rs index cc96004925..1e27b087f1 100644 --- a/tools/twix/src/main.rs +++ b/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, diff --git a/tools/twix/src/panels/automatic_camera_calibration_export.rs b/tools/twix/src/panels/automatic_camera_calibration_export.rs new file mode 100644 index 0000000000..0371c73c1d --- /dev/null +++ b/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, + top_camera: BufferHandle>, + bottom_camera: BufferHandle>, + calibration_corrections: BufferHandle, +} + +impl Panel for AutomaticCameraCalibrationExportPanel { + const NAME: &'static str = "Automatic Calibration"; + + fn new(nao: Arc, _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::(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(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>) { + 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() + )); +} diff --git a/tools/twix/src/panels/manual_camera_calibration.rs b/tools/twix/src/panels/manual_camera_calibration.rs index 34b98a2f0f..3a7cc647e5 100644 --- a/tools/twix/src/panels/manual_camera_calibration.rs +++ b/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, @@ -19,12 +25,9 @@ impl Panel for ManualCalibrationPanel { const NAME: &'static str = "Manual Calibration"; fn new(nao: Arc, _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,8 +85,15 @@ 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") @@ -91,11 +101,11 @@ fn draw_calibration_ui( ); 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") @@ -103,15 +113,15 @@ fn draw_calibration_ui( ); 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()), ); } diff --git a/tools/twix/src/panels/mod.rs b/tools/twix/src/panels/mod.rs index 6cd1a75153..90a9734334 100644 --- a/tools/twix/src/panels/mod.rs +++ b/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;