Center circle detection + calibration integration

Cargo.toml

@@ -15,6 +15,7 @@ members = [
     "crates/context_attribute",
     "crates/control",
     "crates/coordinate_systems",
+    "crates/edge_detection",
     "crates/energy_optimization",
     "crates/filtering",
     "crates/framework",
@@ -121,7 +122,9 @@ hula-types = { path = "tools/hula/types" }
 hulk = { path = "crates/hulk" }
 hulk_manifest = { path = "crates/hulk_manifest" }
 i2cdev = "0.5.1"
-image = "0.24.4"
+image = "0.24.8"
+imageproc = "0.23.0"
+edge_detection = { path = "crates/edge_detection" }
 indicatif = "0.17.2"
 itertools = "0.10.5"
 ittapi = "0.3.3"

crates/calibration/src/center_circle/residuals.rs

@@ -36,22 +36,18 @@ impl CalculateResiduals for CenterCircleResiduals {
             .filter_map(|&point| corrected.pixel_to_ground(point).ok())
             .collect();
 
-        // TODO figure out a better way
-        let has_projection_error =
-            projected_points.len() != measurement.circle_and_points.points.len();
-        if has_projection_error {
+        if projected_points.len() != measurement.circle_and_points.points.len() {
             return Err(ProjectionError::NotOnProjectionPlane);
         }
-        let residual_values = projected_points
+
+        let radial_residuals = projected_points
             .into_iter()
             .map(|projected_point| {
                 (projected_point - projected_center).norm_squared() - radius_squared
             })
             .collect();
 
-        Ok(CenterCircleResiduals {
-            radial_residuals: residual_values,
-        })
+        Ok(CenterCircleResiduals { radial_residuals })
     }
 }
 

crates/control/src/behavior/calibrate.rs

@@ -23,7 +23,7 @@ pub fn execute(
             HeadMotion::LookAt {
                 target,
                 camera: Some(camera),
-                image_region_target: ImageRegion::Bottom,
+                image_region_target: ImageRegion::Center,
             }
         } else {
             HeadMotion::Unstiff

crates/control/src/calibration_controller.rs

@@ -4,10 +4,11 @@ use color_eyre::Result;
 use serde::{Deserialize, Serialize};
 
 use calibration::{
+    center_circle::{measurement::Measurement, residuals::CenterCircleResiduals},
     corrections::Corrections,
-    goal_box::{measurement::Measurement, residuals::GoalBoxResiduals},
     solve,
 };
+
 use context_attribute::context;
 use coordinate_systems::Ground;
 use framework::{AdditionalOutput, MainOutput, PerceptionInput};
@@ -50,8 +51,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 +186,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
@@ -231,7 +238,7 @@ impl CalibrationController {
 
     fn calibrate(&mut self, context: &CycleContext) -> CalibrationState {
         // TODO Handle not enough inner.measurements
-        let solved_result = solve::<GoalBoxResiduals>(
+        let solved_result = solve::<CenterCircleResiduals>(
             Corrections::default(),
             self.inner_states.measurements.clone(),
             *context.field_dimensions,
@@ -276,16 +283,16 @@ fn collect_filtered_values(
 // TODO Add fancier logic to either set this via parameters OR detect the location, walk, etc
 fn generate_look_at_list() -> Vec<(Point2<Ground>, CameraPosition)> {
     let look_at_points: Vec<Point2<Ground>> = vec![
+        point![1.0, -0.2],
+        point![2.0, -0.2],
+        point![2.0, 0.0],
+        point![2.0, 0.2],
+        point![1.0, 0.2],
         point![1.0, 0.0],
-        point![1.0, -0.5],
-        point![3.0, -0.5],
-        point![3.0, 0.0],
-        point![3.0, 0.5],
-        point![1.0, -0.5],
     ];
 
-    look_at_points
+    [CameraPosition::Top, CameraPosition::Bottom]
         .iter()
-        .map(|&point| (point, CameraPosition::Top))
+        .flat_map(|&position| look_at_points.iter().map(move |&point| (point, position)))
         .collect()
 }

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;

crates/edge_detection/Cargo.toml

@@ -0,0 +1,12 @@
+[package]
+name = "edge_detection"
+version.workspace = true
+edition.workspace = true
+license.workspace = true
+homepage.workspace = true
+
+[dependencies]
+image = { workspace = true }
+imageproc = { workspace = true }
+linear_algebra = { workspace = true }
+types = { workspace = true }

crates/edge_detection/src/lib.rs

@@ -0,0 +1,68 @@
+use image::{GrayImage, Luma, RgbImage};
+use imageproc::{edges::canny, filter::gaussian_blur_f32, map::map_colors};
+
+use types::ycbcr422_image::YCbCr422Image;
+
+pub enum EdgeSourceType {
+    DifferenceOfGrayAndRgbRange,
+    LumaOfYCbCr,
+    // TODO Add HSV based approaches - https://github.com/HULKs/hulk/pull/1078, https://github.com/HULKs/hulk/pull/1081
+}
+
+pub fn get_edge_image_canny(
+    gaussian_sigma: f32,
+    canny_low_threshold: f32,
+    canny_high_threshold: f32,
+    image: &YCbCr422Image,
+    source_channel: EdgeSourceType,
+) -> GrayImage {
+    let edges_source = get_edge_source_image(image, source_channel);
+    let blurred = gaussian_blur_f32(&edges_source, gaussian_sigma);
+    canny(&blurred, canny_low_threshold, canny_high_threshold)
+}
+
+pub fn get_edge_source_image(image: &YCbCr422Image, source_type: EdgeSourceType) -> GrayImage {
+    match source_type {
+        EdgeSourceType::DifferenceOfGrayAndRgbRange => {
+            let rgb = RgbImage::from(image);
+
+            let difference = rgb_image_to_difference(&rgb);
+
+            GrayImage::from_vec(
+                difference.width(),
+                difference.height(),
+                difference.into_vec(),
+            )
+            .expect("GrayImage construction after resize failed")
+        }
+        EdgeSourceType::LumaOfYCbCr => {
+            generate_luminance_image(image).expect("Generating luma image failed")
+        }
+    }
+}
+
+fn generate_luminance_image(image: &YCbCr422Image) -> Option<GrayImage> {
+    let grayscale_buffer: Vec<_> = image.iter_pixels().map(|pixel| pixel.y).collect();
+    GrayImage::from_vec(image.width(), image.height(), grayscale_buffer)
+}
+
+fn rgb_image_to_difference(rgb: &RgbImage) -> GrayImage {
+    map_colors(rgb, |color| {
+        Luma([
+            (rgb_pixel_to_gray(&color) - rgb_pixel_to_difference(&color) as i16).clamp(0, 255)
+                as u8,
+        ])
+    })
+}
+
+#[inline]
+fn rgb_pixel_to_gray(rgb: &image::Rgb<u8>) -> i16 {
+    (rgb[0] as i16 + rgb[1] as i16 + rgb[2] as i16) / 3
+}
+
+#[inline]
+fn rgb_pixel_to_difference(rgb: &image::Rgb<u8>) -> u8 {
+    let minimum = rgb.0.iter().min().unwrap();
+    let maximum = rgb.0.iter().max().unwrap();
+    maximum - minimum
+}

crates/hulk_manifest/src/lib.rs

@@ -15,6 +15,7 @@ pub fn collect_hulk_cyclers() -> Result<Cyclers, Error> {
                 nodes: vec![
                     "vision::ball_detection",
                     "vision::calibration_measurement_provider",
+                    "vision::calibration_center_circle_detection",
                     "vision::camera_matrix_extractor",
                     "vision::feet_detection",
                     "vision::field_border_detection",

crates/ransac/Cargo.toml

@@ -8,6 +8,7 @@ homepage.workspace = true
 [dependencies]
 approx = { workspace = true }
 geometry = { workspace = true }
+itertools = { workspace = true }
 linear_algebra = { workspace = true }
 nalgebra = { workspace = true }
 ordered-float = { workspace = true }