From cb2451aab290c1c15690d4e8817b50567f836bc5 Mon Sep 17 00:00:00 2001 From: Harold Ruiter Date: Thu, 17 Jul 2025 14:08:30 -0300 Subject: [PATCH 1/3] Add naive robot detection --- deploy/config/naive_robot_detection.toml | 13 + .../src/vision/ball_detection/classifier.rs | 4 +- yggdrasil/src/vision/line_detection/mod.rs | 2 +- yggdrasil/src/vision/mod.rs | 2 + .../src/vision/naive_robot_detection/mod.rs | 255 ++++++++++++++++++ 5 files changed, 273 insertions(+), 3 deletions(-) create mode 100644 deploy/config/naive_robot_detection.toml create mode 100644 yggdrasil/src/vision/naive_robot_detection/mod.rs diff --git a/deploy/config/naive_robot_detection.toml b/deploy/config/naive_robot_detection.toml new file mode 100644 index 000000000..d9a187d3e --- /dev/null +++ b/deploy/config/naive_robot_detection.toml @@ -0,0 +1,13 @@ +# Minimum number of points in a cluster to be considered a cluster +min_cluster_count = 3 + +# Minimum distance of a point from a line to be considered for a cluster +min_line_distance = 0.01 +# Minimum distance of a point from a ball to be considered for a cluster +min_ball_distance = 0.01 + +# Maximum distance between two points to be considered a cluster +max_cluster_distance = 0.2 + +# Maximum width for a cluster to be used for robot detection +max_cluster_width = 0.5 diff --git a/yggdrasil/src/vision/ball_detection/classifier.rs b/yggdrasil/src/vision/ball_detection/classifier.rs index f0f95a936..24cf3d87a 100644 --- a/yggdrasil/src/vision/ball_detection/classifier.rs +++ b/yggdrasil/src/vision/ball_detection/classifier.rs @@ -94,7 +94,7 @@ fn init_ball_tracker(mut commands: Commands, config: Res) { commands.insert_resource(ball_tracker); } -pub(super) struct BallClassifierModel; +pub struct BallClassifierModel; impl MlModel for BallClassifierModel { type Inputs = Vec; @@ -151,7 +151,7 @@ fn update_ball_tracker(mut ball_tracker: ResMut, odometry: Res( +pub fn classify_balls( ctx: DebugContext, cycle: Res, mut commands: Commands, diff --git a/yggdrasil/src/vision/line_detection/mod.rs b/yggdrasil/src/vision/line_detection/mod.rs index 581f8c1c3..714815148 100644 --- a/yggdrasil/src/vision/line_detection/mod.rs +++ b/yggdrasil/src/vision/line_detection/mod.rs @@ -185,7 +185,7 @@ impl LineCandidate { #[derive(Component)] pub struct LineTaskHandle(Task<(Vec, Vec>)>); -fn detect_lines_system( +pub fn detect_lines_system( mut commands: Commands, scan_lines: Res>, camera_matrix: Res>, diff --git a/yggdrasil/src/vision/mod.rs b/yggdrasil/src/vision/mod.rs index 5d6aa1b3e..e76519c9e 100644 --- a/yggdrasil/src/vision/mod.rs +++ b/yggdrasil/src/vision/mod.rs @@ -7,6 +7,7 @@ pub mod camera; pub mod color; pub mod field_boundary; pub mod line_detection; +pub mod naive_robot_detection; pub mod referee; pub mod robot_detection; pub mod scan_grid; @@ -28,6 +29,7 @@ impl PluginGroup for VisionPlugins { .add(line_detection::LineDetectionPlugin::::default()) .add(field_boundary::FieldBoundaryPlugin) .add(ball_detection::BallDetectionPlugin) + .add(naive_robot_detection::NaiveRobotDetectionPlugin) // .add(robot_detection::RobotDetectionPlugin) .add(referee::VisualRefereePlugin); diff --git a/yggdrasil/src/vision/naive_robot_detection/mod.rs b/yggdrasil/src/vision/naive_robot_detection/mod.rs new file mode 100644 index 000000000..5a4d3244e --- /dev/null +++ b/yggdrasil/src/vision/naive_robot_detection/mod.rs @@ -0,0 +1,255 @@ +use bevy::prelude::*; +use heimdall::{CameraLocation, CameraMatrix, Top}; +use nalgebra::Point2; +use odal::Config; +use serde::{Deserialize, Serialize}; + +use crate::{ + core::debug::DebugContext, + localization::RobotPose, + nao::Cycle, + prelude::ConfigExt, + vision::{ + ball_detection::{ball_tracker::BallTracker, classifier::classify_balls}, + line_detection::{DetectedLines, detect_lines_system, line::LineSegment2}, + scan_lines::{RegionColor, ScanLines}, + }, +}; + +#[derive(Resource, Serialize, Deserialize, Debug, Clone)] +#[serde(deny_unknown_fields)] +struct NaiveRobotDetectionConfig { + /// Minimum number of points in a cluster to be considered a cluster + min_cluster_count: usize, + + /// Minimum distance of a point from a line to be considered for a cluster + min_line_distance: f32, + /// Minimum distance of a point from a ball to be considered for a cluster + min_ball_distance: f32, + + /// Maximum distance between two points to be considered a cluster + max_cluster_distance: f32, + + /// Maximum width for a cluster to be used for robot detection + max_cluster_width: f32, +} + +impl Config for NaiveRobotDetectionConfig { + const PATH: &'static str = "naive_robot_detection.toml"; +} + +/// Plugin for naive robot detection using the vertical scan lines. +pub struct NaiveRobotDetectionPlugin; + +impl Plugin for NaiveRobotDetectionPlugin { + fn build(&self, app: &mut App) { + app.init_config::() + .init_resource::() + .add_systems(PostStartup, setup_visualizations) + .add_systems( + Update, + ( + detect_robots + .after(classify_balls::) + .after(detect_lines_system::) + .run_if(resource_exists_and_changed::>), + visualize_detections, + ) + .chain(), + ); + } +} + +struct Cluster { + /// Center of the cluster in field coordinates + center: Point2, + /// Width of the cluster + width: f32, + /// Number of points in the cluster + count: usize, +} + +impl Cluster { + fn from_points(points: Vec>) -> Self { + let center = points + .iter() + .fold(Point2::::origin(), |acc, point| acc + point.coords) + / points.len() as f32; + + let width = points + .iter() + .map(|point| nalgebra::distance(¢er, point)) + .fold(0.0, f32::max); + + let points = points.len(); + + Self { + center, + width, + count: points, + } + } +} + +#[derive(Resource, Debug, Default, Clone)] +struct NaiveDetectedRobots { + positions: Vec>, + cycle: Cycle, +} + +fn detect_robots( + scan_lines: Res>, + camera_matrix: Res>, + detected_lines: Query<&DetectedLines>, + ball_tracker: Res, + config: Res, + mut detected_robots: ResMut, +) { + // Get the cluster points from the scan lines. + // + // Points are ordered by the vertical scan lines and represent the lowest + // point of the lowest white region in each vertical scan line + let cluster_points = get_cluster_points(&scan_lines); + + // project the cluster points to the ground plane + let mut projected_points = project_cluster_points(cluster_points, &camera_matrix); + + let lines = detected_lines + .iter() + .flat_map(|lines| project_lines(lines, &camera_matrix)) + .collect::>(); + + projected_points.retain(|point| { + lines + .iter() + .all(|line| line.distance_to_point(*point) > config.min_line_distance) + && ball_tracker + .stationary_ball() + .is_none_or(|ball| nalgebra::distance(&ball, point) > config.min_ball_distance) + }); + + // filter out points that are in lines or balls + + let mut clusters = create_clusters(projected_points, &config); + + clusters.retain(|cluster| { + // filter out clusters that are too small or too large + cluster.width < config.max_cluster_width && cluster.count > config.min_cluster_count + }); + + *detected_robots = NaiveDetectedRobots { + positions: clusters.into_iter().map(|cluster| cluster.center).collect(), + cycle: scan_lines.image().cycle(), + }; +} + +fn setup_visualizations(dbg: DebugContext) { + dbg.log_static( + Top::make_entity_path("naive_detected_robots"), + &rerun::Ellipsoids3D::update_fields().with_colors([(0, 0, 255)]), + ); +} + +fn visualize_detections( + dbg: DebugContext, + detected_robots: Res, + robot_pose: Res, +) { + dbg.log_with_cycle( + Top::make_entity_path("naive_detected_robots"), + detected_robots.cycle, + &rerun::Ellipsoids3D::from_centers_and_radii( + detected_robots.positions.iter().map(|point| { + let point = robot_pose.robot_to_world(point); + (point.coords.x, point.coords.y, 0.1) + }), + (0..detected_robots.positions.len()).map(|_| 0.2), + ), + ); +} + +fn get_cluster_points(scan_lines: &ScanLines) -> Vec> { + let mut cluster_points = vec![]; + + let mut current_fixed_point: Option = None; + let mut lowest_white_point_in_region: Option> = None; + for region in scan_lines.vertical().regions() { + // every vertical scan line we can get a new cluster point + if current_fixed_point.is_none_or(|current| current != region.fixed_point()) { + if let Some(lowest_white_point) = lowest_white_point_in_region { + cluster_points.push(lowest_white_point); + } + + current_fixed_point = Some(region.fixed_point()); + lowest_white_point_in_region = None; + } + + let end_point = Point2::new(region.fixed_point() as f32, region.end_point() as f32); + // we consider the lowest white region for the cluster point + if matches!(region.color(), RegionColor::WhiteOrBlack) + && lowest_white_point_in_region.is_none_or(|point| point.y < end_point.y) + { + lowest_white_point_in_region = Some(end_point); + } + } + + if let Some(point) = lowest_white_point_in_region { + cluster_points.push(point); + } + + cluster_points +} + +fn project_cluster_points( + cluster_points: Vec>, + camera_matrix: &CameraMatrix, +) -> Vec> { + cluster_points + .into_iter() + .filter_map(|point| camera_matrix.pixel_to_ground(point, 0.0).ok()) + .map(|point| point.xy()) + .collect::>() +} + +fn create_clusters( + cluster_points: Vec>, + config: &NaiveRobotDetectionConfig, +) -> Vec { + let mut clusters = vec![]; + + let mut current_cluster: Vec> = vec![]; + let mut last_point: Option> = None; + for point in cluster_points { + // if the current cluster is empty, we can start a new one + let Some(last) = last_point else { + current_cluster.push(point); + last_point = Some(point); + continue; + }; + + if nalgebra::distance(&point, &last) < config.max_cluster_distance { + current_cluster.push(point); + } else { + clusters.push(Cluster::from_points(std::mem::take(&mut current_cluster))); + current_cluster.push(point); + } + } + + if !current_cluster.is_empty() { + clusters.push(Cluster::from_points(current_cluster)); + } + + clusters +} + +fn project_lines(lines: &DetectedLines, camera_matrix: &CameraMatrix) -> Vec { + lines + .segments + .iter() + .filter_map(|line| { + let start = camera_matrix.pixel_to_ground(line.start, 0.0).ok()?; + let end = camera_matrix.pixel_to_ground(line.end, 0.0).ok()?; + Some(LineSegment2::new(start.xy(), end.xy())) + }) + .collect() +} From 84e07b5a1d107153868dfd019bea205612136696 Mon Sep 17 00:00:00 2001 From: Harold Ruiter Date: Thu, 17 Jul 2025 20:50:03 -0300 Subject: [PATCH 2/3] detect stuff but not that good --- deploy/config/naive_robot_detection.toml | 14 +++-- .../src/vision/naive_robot_detection/mod.rs | 59 +++++++++++-------- 2 files changed, 43 insertions(+), 30 deletions(-) diff --git a/deploy/config/naive_robot_detection.toml b/deploy/config/naive_robot_detection.toml index d9a187d3e..a92431b88 100644 --- a/deploy/config/naive_robot_detection.toml +++ b/deploy/config/naive_robot_detection.toml @@ -1,13 +1,15 @@ # Minimum number of points in a cluster to be considered a cluster -min_cluster_count = 3 +min_cluster_count = 5 # Minimum distance of a point from a line to be considered for a cluster -min_line_distance = 0.01 +min_line_distance = 0.1 # Minimum distance of a point from a ball to be considered for a cluster -min_ball_distance = 0.01 +min_ball_distance = 0.25 # Maximum distance between two points to be considered a cluster -max_cluster_distance = 0.2 +max_cluster_distance = 0.25 -# Maximum width for a cluster to be used for robot detection -max_cluster_width = 0.5 +# Minimum radius for a cluster to be used for robot detection +min_cluster_radius = 0.05 +# Maximum radius for a cluster to be used for robot detection +max_cluster_radius = 0.1 diff --git a/yggdrasil/src/vision/naive_robot_detection/mod.rs b/yggdrasil/src/vision/naive_robot_detection/mod.rs index 5a4d3244e..6fb72a872 100644 --- a/yggdrasil/src/vision/naive_robot_detection/mod.rs +++ b/yggdrasil/src/vision/naive_robot_detection/mod.rs @@ -30,8 +30,10 @@ struct NaiveRobotDetectionConfig { /// Maximum distance between two points to be considered a cluster max_cluster_distance: f32, - /// Maximum width for a cluster to be used for robot detection - max_cluster_width: f32, + /// Minimum radius for a cluster to be used for robot detection + min_cluster_radius: f32, + /// Maximum radius for a cluster to be used for robot detection + max_cluster_radius: f32, } impl Config for NaiveRobotDetectionConfig { @@ -97,14 +99,25 @@ struct NaiveDetectedRobots { cycle: Cycle, } +#[allow(clippy::too_many_arguments)] fn detect_robots( + dbg: DebugContext, + robot_pose: Res, scan_lines: Res>, camera_matrix: Res>, - detected_lines: Query<&DetectedLines>, + detected_lines: Query<&DetectedLines, Added>, ball_tracker: Res, config: Res, mut detected_robots: ResMut, + mut segments: Local>, ) { + if !detected_lines.is_empty() { + *segments = detected_lines + .iter() + .flat_map(|line| line.segments.clone()) + .collect::>(); + } + // Get the cluster points from the scan lines. // // Points are ordered by the vertical scan lines and represent the lowest @@ -114,13 +127,8 @@ fn detect_robots( // project the cluster points to the ground plane let mut projected_points = project_cluster_points(cluster_points, &camera_matrix); - let lines = detected_lines - .iter() - .flat_map(|lines| project_lines(lines, &camera_matrix)) - .collect::>(); - projected_points.retain(|point| { - lines + segments .iter() .all(|line| line.distance_to_point(*point) > config.min_line_distance) && ball_tracker @@ -128,13 +136,25 @@ fn detect_robots( .is_none_or(|ball| nalgebra::distance(&ball, point) > config.min_ball_distance) }); + dbg.log_with_cycle( + Top::make_entity_path("naive_detected_robots/filtered_points"), + detected_robots.cycle, + &rerun::Points3D::new(projected_points.iter().map(|point| { + let point = robot_pose.robot_to_world(point); + (point.coords.x, point.coords.y, 0.1) + })) + .with_colors((0..projected_points.len()).map(|_| (255, 0, 0))), + ); + // filter out points that are in lines or balls let mut clusters = create_clusters(projected_points, &config); clusters.retain(|cluster| { // filter out clusters that are too small or too large - cluster.width < config.max_cluster_width && cluster.count > config.min_cluster_count + cluster.width > config.min_cluster_radius + && cluster.width < config.max_cluster_radius + && cluster.count > config.min_cluster_count }); *detected_robots = NaiveDetectedRobots { @@ -221,13 +241,16 @@ fn create_clusters( let mut last_point: Option> = None; for point in cluster_points { // if the current cluster is empty, we can start a new one - let Some(last) = last_point else { + let Some(_) = last_point else { current_cluster.push(point); last_point = Some(point); continue; }; - if nalgebra::distance(&point, &last) < config.max_cluster_distance { + if current_cluster + .iter() + .any(|p| nalgebra::distance(&point, p) < config.max_cluster_distance) + { current_cluster.push(point); } else { clusters.push(Cluster::from_points(std::mem::take(&mut current_cluster))); @@ -241,15 +264,3 @@ fn create_clusters( clusters } - -fn project_lines(lines: &DetectedLines, camera_matrix: &CameraMatrix) -> Vec { - lines - .segments - .iter() - .filter_map(|line| { - let start = camera_matrix.pixel_to_ground(line.start, 0.0).ok()?; - let end = camera_matrix.pixel_to_ground(line.end, 0.0).ok()?; - Some(LineSegment2::new(start.xy(), end.xy())) - }) - .collect() -} From 955b07d3ebbc3a3345e5a595d6095aebd6527801 Mon Sep 17 00:00:00 2001 From: Harold Ruiter Date: Thu, 17 Jul 2025 20:54:26 -0300 Subject: [PATCH 3/3] recalibrate daphne --- deploy/config/overlay/daphne/yggdrasil.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/deploy/config/overlay/daphne/yggdrasil.toml b/deploy/config/overlay/daphne/yggdrasil.toml index 654e62725..68a928a26 100644 --- a/deploy/config/overlay/daphne/yggdrasil.toml +++ b/deploy/config/overlay/daphne/yggdrasil.toml @@ -1,6 +1,6 @@ [camera.top.calibration] # The extrinsic rotation for the camera in degrees, xyz euler angles. -extrinsic_rotation = [0.00, -4.93, -2.02] +extrinsic_rotation = [2.25, -5.75, -0.86] # The focal length for the camera in pixels. focal_lengths = [608, 609] # The optical center of the camera sensor in pixels.