diff --git a/Cargo.lock b/Cargo.lock index ac71674733..8a5018f906 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -6086,7 +6086,7 @@ dependencies = [ [[package]] name = "twix" -version = "0.7.0" +version = "0.7.1" dependencies = [ "aliveness", "argument_parsers", diff --git a/crates/types/src/ycbcr422_image.rs b/crates/types/src/ycbcr422_image.rs index 8ed608e861..45a4c894ce 100644 --- a/crates/types/src/ycbcr422_image.rs +++ b/crates/types/src/ycbcr422_image.rs @@ -6,11 +6,12 @@ use std::{ }; use color_eyre::eyre::{self, WrapErr}; +use geometry::circle::Circle; use image::{io::Reader, RgbImage}; use serde::{Deserialize, Serialize}; use coordinate_systems::Pixel; -use linear_algebra::Point2; +use linear_algebra::{vector, Point2}; use path_serde::{PathDeserialize, PathIntrospect, PathSerialize}; use crate::{ @@ -18,6 +19,8 @@ use crate::{ jpeg::JpegImage, }; +pub const SAMPLE_SIZE: usize = 32; + #[derive( Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathIntrospect, PathDeserialize, )] @@ -242,4 +245,22 @@ impl YCbCr422Image { ycbcr444 }) } + + pub fn sample_grayscale(&self, candidate: Circle) -> Sample { + let top_left = candidate.center - vector![candidate.radius, candidate.radius]; + let image_pixels_per_sample_pixel = candidate.radius * 2.0 / SAMPLE_SIZE as f32; + + let mut sample = Sample::default(); + for (y, column) in sample.iter_mut().enumerate() { + for (x, pixel) in column.iter_mut().enumerate() { + let x = (top_left.x() + x as f32 * image_pixels_per_sample_pixel) as u32; + let y = (top_left.y() + y as f32 * image_pixels_per_sample_pixel) as u32; + *pixel = self.try_at(x, y).map_or(128.0, |pixel| pixel.y as f32); + } + } + + sample + } } + +pub type Sample = [[f32; SAMPLE_SIZE]; SAMPLE_SIZE]; diff --git a/crates/vision/src/ball_detection.rs b/crates/vision/src/ball_detection.rs index b9038cdb32..272b2a2361 100644 --- a/crates/vision/src/ball_detection.rs +++ b/crates/vision/src/ball_detection.rs @@ -14,12 +14,9 @@ use types::{ multivariate_normal_distribution::MultivariateNormalDistribution, parameters::BallDetectionParameters, perspective_grid_candidates::PerspectiveGridCandidates, - ycbcr422_image::YCbCr422Image, + ycbcr422_image::{Sample, YCbCr422Image, SAMPLE_SIZE}, }; -pub const SAMPLE_SIZE: usize = 32; -pub type Sample = [[f32; SAMPLE_SIZE]; SAMPLE_SIZE]; - struct NeuralNetworks { preclassifier: CompiledNN, classifier: CompiledNN, @@ -184,22 +181,6 @@ fn position_sample(network: &mut CompiledNN, sample: &Sample) -> Circle { } } -fn sample_grayscale(image: &YCbCr422Image, candidate: Circle) -> Sample { - let top_left = candidate.center - vector![candidate.radius, candidate.radius]; - let image_pixels_per_sample_pixel = candidate.radius * 2.0 / SAMPLE_SIZE as f32; - - let mut sample = Sample::default(); - for (y, column) in sample.iter_mut().enumerate() { - for (x, pixel) in column.iter_mut().enumerate() { - let x = (top_left.x() + x as f32 * image_pixels_per_sample_pixel) as u32; - let y = (top_left.y() + y as f32 * image_pixels_per_sample_pixel) as u32; - *pixel = image.try_at(x, y).map_or(128.0, |pixel| pixel.y as f32); - } - } - - sample -} - fn evaluate_candidates( candidates: &[Circle], image: &YCbCr422Image, @@ -221,7 +202,7 @@ fn evaluate_candidates( center: candidate.center, radius: candidate.radius * ball_radius_enlargement_factor, }; - let sample = sample_grayscale(image, enlarged_candidate); + let sample = image.sample_grayscale(enlarged_candidate); let preclassifier_confidence = preclassify_sample(preclassifier, &sample); let mut classifier_confidence = None; @@ -401,13 +382,12 @@ mod tests { fn preclassify_ball() { let mut network = CompiledNN::default(); network.compile(CLASSIFIER_PATH); - let sample = sample_grayscale( - &YCbCr422Image::load_from_444_png(Path::new(BALL_SAMPLE_PATH)).unwrap(), - Circle { + let sample = YCbCr422Image::load_from_444_png(Path::new(BALL_SAMPLE_PATH)) + .unwrap() + .sample_grayscale(Circle { center: point![16.0, 16.0], radius: 16.0, - }, - ); + }); let confidence = preclassify_sample(&mut network, &sample); println!("{confidence:?}"); @@ -418,13 +398,12 @@ mod tests { fn classify_ball() { let mut network = CompiledNN::default(); network.compile(PRECLASSIFIER_PATH); - let sample = sample_grayscale( - &YCbCr422Image::load_from_444_png(Path::new(BALL_SAMPLE_PATH)).unwrap(), - Circle { + let sample = YCbCr422Image::load_from_444_png(Path::new(BALL_SAMPLE_PATH)) + .unwrap() + .sample_grayscale(Circle { center: point![16.0, 16.0], radius: 16.0, - }, - ); + }); let confidence = classify_sample(&mut network, &sample); println!("{confidence:?}"); @@ -435,13 +414,12 @@ mod tests { fn position_ball() { let mut network = CompiledNN::default(); network.compile(POSITIONER_PATH); - let sample = sample_grayscale( - &YCbCr422Image::load_from_444_png(Path::new(BALL_SAMPLE_PATH)).unwrap(), - Circle { + let sample = YCbCr422Image::load_from_444_png(Path::new(BALL_SAMPLE_PATH)) + .unwrap() + .sample_grayscale(Circle { center: point![16.0, 16.0], radius: 16.0, - }, - ); + }); let circle = position_sample(&mut network, &sample); assert_relative_eq!( diff --git a/tools/twix/Cargo.toml b/tools/twix/Cargo.toml index 7892977458..83ae3e028d 100644 --- a/tools/twix/Cargo.toml +++ b/tools/twix/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "twix" -version = "0.7.0" +version = "0.7.1" edition.workspace = true license.workspace = true homepage.workspace = true diff --git a/tools/twix/src/main.rs b/tools/twix/src/main.rs index 81c095987d..fe4e82582f 100644 --- a/tools/twix/src/main.rs +++ b/tools/twix/src/main.rs @@ -34,9 +34,9 @@ use log::error; use nao::Nao; use panel::Panel; use panels::{ - BehaviorSimulatorPanel, EnumPlotPanel, ImageColorSelectPanel, ImagePanel, ImageSegmentsPanel, - LookAtPanel, ManualCalibrationPanel, MapPanel, ParameterPanel, PlotPanel, RemotePanel, - TextPanel, VisionTunerPanel, + BallCandidatePanel, BehaviorSimulatorPanel, EnumPlotPanel, ImageColorSelectPanel, ImagePanel, + ImageSegmentsPanel, LookAtPanel, ManualCalibrationPanel, MapPanel, ParameterPanel, PlotPanel, + RemotePanel, TextPanel, VisionTunerPanel, }; use repository::{get_repository_root, Repository}; @@ -155,6 +155,7 @@ impl ReachableNaos { } impl_selectable_panel!( + BallCandidatePanel, BehaviorSimulatorPanel, ImagePanel, ImageSegmentsPanel, diff --git a/tools/twix/src/panels/ball_candidates.rs b/tools/twix/src/panels/ball_candidates.rs new file mode 100644 index 0000000000..b97f0f9fcb --- /dev/null +++ b/tools/twix/src/panels/ball_candidates.rs @@ -0,0 +1,208 @@ +use std::sync::Arc; + +use coordinate_systems::Pixel; +use eframe::{ + egui::{Color32, Pos2, Rect, Response, Stroke, Ui, Vec2, Widget}, + emath::RectTransform, +}; +use geometry::circle::Circle; +use linear_algebra::{point, vector}; +use serde_json::{json, Value}; +use types::{ + ball_detection::CandidateEvaluation, + ycbcr422_image::{YCbCr422Image, SAMPLE_SIZE}, +}; + +use crate::{ + nao::Nao, + panel::Panel, + twix_painter::{Orientation, TwixPainter}, + value_buffer::BufferHandle, +}; + +use super::image::cycler_selector::{VisionCycler, VisionCyclerSelector}; + +pub struct BallCandidatePanel { + nao: Arc, + cycler: VisionCycler, + ball_radius_enlargement_factor: BufferHandle, + ball_candidates: BufferHandle>>, + image: BufferHandle, +} + +impl Panel for BallCandidatePanel { + const NAME: &'static str = "Ball Candidates"; + + fn new(nao: Arc, value: Option<&Value>) -> Self { + let cycler = value + .and_then(|value| { + let string = value.get("cycler")?.as_str()?; + VisionCycler::try_from(string).ok() + }) + .unwrap_or(VisionCycler::Top); + + let cycler_path = cycler.as_snake_case_path(); + let ball_radius_enlargement_factor = nao.subscribe_value(format!( + "parameters.ball_detection.{cycler_path}.ball_radius_enlargement_factor", + )); + let cycler_path = cycler.as_path(); + let ball_candidates = + nao.subscribe_value(format!("{cycler_path}.additional_outputs.ball_candidates")); + let image = nao.subscribe_value(format!("{cycler_path}.main_outputs.image")); + Self { + nao, + cycler, + ball_radius_enlargement_factor, + ball_candidates, + image, + } + } + + fn save(&self) -> Value { + json!({ + "cycler": self.cycler.as_path(), + }) + } +} + +impl Widget for &mut BallCandidatePanel { + fn ui(self, ui: &mut Ui) -> Response { + ui.vertical(|ui| { + ui.horizontal(|ui| { + let mut cycler_selector = VisionCyclerSelector::new(&mut self.cycler); + if cycler_selector.ui(ui).changed() { + self.resubscribe(); + } + }); + ui.separator(); + if let Some((ball_radius_enlargement_factor, ball_candidates, image)) = self + .ball_radius_enlargement_factor + .get_last_value() + .ok() + .flatten() + .and_then(|ball_radius_enlargement_factor| { + self.ball_candidates + .get_last_value() + .ok() + .flatten() + .flatten() + .map(|ball_candidates| (ball_radius_enlargement_factor, ball_candidates)) + }) + .and_then(|(ball_radius_enlargement_factor, ball_candidates)| { + self.image + .get_last_value() + .ok() + .flatten() + .map(|image| (ball_radius_enlargement_factor, ball_candidates, image)) + }) + { + ui.horizontal_wrapped(|ui| { + for candidate in ball_candidates { + ui.add(CandidateSample { + ball_radius_enlargement_factor, + candidate, + image: image.clone(), + }); + } + }); + } else { + ui.label("Some outputs are missing"); + } + }) + .response + } +} + +impl BallCandidatePanel { + fn resubscribe(&mut self) { + let cycler_path = self.cycler.as_snake_case_path(); + self.ball_radius_enlargement_factor = self.nao.subscribe_value(format!( + "parameters.ball_detection.{cycler_path}.ball_radius_enlargement_factor", + )); + let cycler_path = self.cycler.as_path(); + self.ball_candidates = self + .nao + .subscribe_value(format!("{cycler_path}.additional_outputs.ball_candidates")); + self.image = self + .nao + .subscribe_value(format!("{cycler_path}.main_outputs.image")); + } +} + +struct CandidateSample { + ball_radius_enlargement_factor: f32, + candidate: CandidateEvaluation, + image: YCbCr422Image, +} + +impl Widget for CandidateSample { + fn ui(self, ui: &mut Ui) -> Response { + let enlarged_candidate = Circle { + center: self.candidate.candidate_circle.center, + radius: self.candidate.candidate_circle.radius * self.ball_radius_enlargement_factor, + }; + + let sample = self.image.sample_grayscale(enlarged_candidate); + + const SAMPLE_SIZE_F32: f32 = SAMPLE_SIZE as f32; + const SCALING: f32 = 3.0; + ui.allocate_ui( + Vec2::new(SAMPLE_SIZE_F32 * SCALING, SAMPLE_SIZE_F32 * SCALING), + |ui| { + let (response, painter) = TwixPainter::::allocate( + ui, + vector![SAMPLE_SIZE_F32, SAMPLE_SIZE_F32], + point![0.0, 0.0], + Orientation::LeftHanded, + ); + + for (y, sample_row) in sample.iter().enumerate() { + let y = y as f32; + for (x, sample_value) in sample_row.iter().enumerate() { + let x = x as f32; + painter.rect_filled( + point![x, y], + point![x + 1.0, y + 1.0], + Color32::from_gray(*sample_value as u8), + ); + } + } + + if let Some(corrected_circle) = self.candidate.corrected_circle { + let candidate_circle = self.candidate.candidate_circle; + let transform = RectTransform::from_to( + Rect::from_center_size( + Pos2::new(candidate_circle.center.x(), candidate_circle.center.y()), + Vec2::new( + candidate_circle.radius * 2.0 * self.ball_radius_enlargement_factor, + candidate_circle.radius * 2.0 * self.ball_radius_enlargement_factor, + ), + ), + Rect::from_min_size( + Pos2::ZERO, + Vec2::new(SAMPLE_SIZE_F32, SAMPLE_SIZE_F32), + ), + ); + let corrected_center_in_sample = transform.transform_pos(Pos2::new( + corrected_circle.center.x(), + corrected_circle.center.y(), + )); + let corrected_center_in_sample = + point![corrected_center_in_sample.x, corrected_center_in_sample.y]; + let corrected_radius_in_sample = corrected_circle.radius + / (self.candidate.candidate_circle.radius + * self.ball_radius_enlargement_factor) + * (SAMPLE_SIZE_F32 / 2.0); + painter.circle_stroke( + corrected_center_in_sample, + corrected_radius_in_sample, + Stroke::new(0.5, Color32::GREEN), + ); + } + + response + }, + ) + .response + } +} diff --git a/tools/twix/src/panels/image/overlays/ball_detection.rs b/tools/twix/src/panels/image/overlays/ball_detection.rs index b0b4d2e80d..955acd5b89 100644 --- a/tools/twix/src/panels/image/overlays/ball_detection.rs +++ b/tools/twix/src/panels/image/overlays/ball_detection.rs @@ -2,6 +2,7 @@ use color_eyre::Result; use coordinate_systems::Pixel; use eframe::epaint::{Color32, Stroke}; use geometry::circle::Circle; +use linear_algebra::vector; use types::ball_detection::{BallPercept, CandidateEvaluation}; use crate::{ @@ -14,6 +15,7 @@ pub struct BallDetection { balls: BufferHandle>>, filtered_balls: BufferHandle>>>, ball_candidates: BufferHandle>>, + ball_radius_enlargement_factor: BufferHandle, } impl Overlay for BallDetection { @@ -25,6 +27,7 @@ impl Overlay for BallDetection { VisionCycler::Bottom => "bottom", }; let cycler_path = selected_cycler.as_path(); + let cycler_path_snake_case = selected_cycler.as_snake_case_path(); Self { balls: nao.subscribe_value(format!("{cycler_path}.main_outputs.balls")), filtered_balls: nao.subscribe_value(format!( @@ -32,6 +35,9 @@ impl Overlay for BallDetection { )), ball_candidates: nao .subscribe_value(format!("{cycler_path}.additional_outputs.ball_candidates")), + ball_radius_enlargement_factor: nao.subscribe_value(format!( + "parameters.ball_detection.{cycler_path_snake_case}.ball_radius_enlargement_factor", + )), } } @@ -42,13 +48,28 @@ impl Overlay for BallDetection { } } - if let Some(ball_candidates) = self.ball_candidates.get_last_value()?.flatten() { + if let (Some(ball_candidates), Some(ball_radius_enlargement_factor)) = ( + self.ball_candidates.get_last_value()?.flatten(), + self.ball_radius_enlargement_factor.get_last_value()?, + ) { for candidate in ball_candidates.iter() { let circle = candidate.candidate_circle; painter.circle_stroke( circle.center, circle.radius, - Stroke::new(2.0, Color32::BLUE), + Stroke::new(1.0, Color32::BLUE), + ); + + let enlarged_candidate = Circle { + center: candidate.candidate_circle.center, + radius: candidate.candidate_circle.radius * ball_radius_enlargement_factor, + }; + painter.rect_stroke( + enlarged_candidate.center + - vector![enlarged_candidate.radius, enlarged_candidate.radius], + enlarged_candidate.center + + vector![enlarged_candidate.radius, enlarged_candidate.radius], + Stroke::new(1.0, Color32::DARK_BLUE), ); } for candidate in ball_candidates.iter() { diff --git a/tools/twix/src/panels/mod.rs b/tools/twix/src/panels/mod.rs index 0ed21ade2b..6cd1a75153 100644 --- a/tools/twix/src/panels/mod.rs +++ b/tools/twix/src/panels/mod.rs @@ -1,3 +1,4 @@ +mod ball_candidates; mod behavior_simulator; mod enum_plot; mod image; @@ -12,9 +13,10 @@ mod remote; mod text; mod vision_tuner; -pub use self::behavior_simulator::BehaviorSimulatorPanel; -pub use self::image::ImagePanel; +pub use ball_candidates::BallCandidatePanel; +pub use behavior_simulator::BehaviorSimulatorPanel; pub use enum_plot::EnumPlotPanel; +pub use image::ImagePanel; pub use image_color_select::ImageColorSelectPanel; pub use image_segments::ImageSegmentsPanel; pub use look_at::LookAtPanel;