Add panel for displaying ransac features

Author: julianschuler

Cargo.lock

@@ -28,6 +28,7 @@ object_detection = { workspace = true }
 parameters = { workspace = true }
 path_serde = { workspace = true }
 projection = { workspace = true }
+ransac = { workspace = true }
 serde = { workspace = true }
 serde_json = { workspace = true }
 spl_network = { workspace = true }

crates/hulk/Cargo.toml

@@ -17,10 +17,11 @@ geometry = { workspace = true }
 hardware = { workspace = true }
 ittapi = { workspace = true }
 linear_algebra = { workspace = true }
+path_serde = { workspace = true }
 parameters = { workspace = true }
 projection = { workspace = true }
+ransac = { workspace = true }
 serde = { workspace = true }
-path_serde = { workspace = true }
 tokio = { workspace = true }
 types = { workspace = true }
 vision = { workspace = true }

crates/hulk_imagine/Cargo.toml

@@ -31,6 +31,7 @@ object_detection = { workspace = true }
 parameters = { workspace = true }
 path_serde = { workspace = true }
 projection = { workspace = true }
+ransac = { workspace = true }
 serde = { workspace = true }
 serde_json = { workspace = true }
 spl_network = { workspace = true }

crates/hulk_replayer/Cargo.toml

@@ -13,3 +13,4 @@ nalgebra = { workspace = true }
 ordered-float = { workspace = true }
 rand = { workspace = true }
 rand_chacha = { workspace = true }
+serde = { workspace = true }

crates/ransac/Cargo.toml

@@ -9,8 +9,9 @@ use geometry::{
 use linear_algebra::{Point2, Rotation2};
 use ordered_float::NotNan;
 use rand::{seq::SliceRandom, Rng};
+use serde::{Deserialize, Serialize};
 
-#[derive(Default, Debug, PartialEq)]
+#[derive(Clone, Debug, Default, Deserialize, Serialize, PartialEq)]
 pub enum RansacFeature<Frame> {
     #[default]
     None,

crates/ransac/src/lib.rs

@@ -6,7 +6,7 @@ mod segment_merger;
 use std::{collections::HashSet, ops::Range};
 
 use color_eyre::Result;
-use geometry::line_segment::LineSegment;
+use geometry::{line::Line, line_segment::LineSegment, two_lines::TwoLines};
 use itertools::Itertools;
 use rand::SeedableRng;
 use rand_chacha::ChaChaRng;
@@ -17,7 +17,7 @@ use coordinate_systems::{Ground, Pixel};
 use framework::{AdditionalOutput, MainOutput};
 use linear_algebra::{distance, Point2};
 use projection::{camera_matrix::CameraMatrix, Projection};
-use ransac::{Ransac, RansacLineSegment};
+use ransac::{Ransac, RansacFeature, RansacLineSegment};
 use types::{
     filtered_segments::FilteredSegments,
     image_segments::GenericSegment,
@@ -44,6 +44,7 @@ pub struct CycleContext {
         AdditionalOutput<Vec<(LineSegment<Pixel>, LineDiscardReason)>, "discarded_lines">,
     filtered_segments_output:
         AdditionalOutput<Vec<GenericSegment>, "line_detection.filtered_segments">,
+    detected_features: AdditionalOutput<Vec<RansacFeature<Pixel>>, "detected_features">,
 
     use_horizontal_segments:
         Parameter<bool, "line_detection.$cycler_instance.use_horizontal_segments">,
@@ -160,21 +161,23 @@ impl LineDetection {
                 .unzip();
 
         let mut ransac = Ransac::new(line_points);
+        let mut detected_features = Vec::new();
         let mut line_segments = Vec::new();
         let mut discarded_line_segments = Vec::new();
 
         for _ in 0..*context.maximum_number_of_features {
             if ransac.unused_points.len() < *context.minimum_number_of_points_on_line {
                 break;
             }
-            let ransac_feature = ransac.next_feature(
+            let ransac_result = ransac.next_feature(
                 &mut self.random_state,
                 *context.ransac_iterations,
                 *context.maximum_fit_distance_in_ground,
                 *context.maximum_fit_distance_in_ground + *context.margin_for_point_inclusion,
             );
+            detected_features.push(ransac_result.feature.clone());
 
-            for line_segment in ransac_feature {
+            for line_segment in ransac_result {
                 let RansacLineSegment {
                     line_segment,
                     sorted_used_points,
@@ -231,6 +234,40 @@ impl LineDetection {
             }
         }
 
+        context.detected_features.fill_if_subscribed(|| {
+            detected_features
+                .into_iter()
+                .map(|feature| match feature {
+                    RansacFeature::None => RansacFeature::None,
+                    RansacFeature::Line(line) => RansacFeature::Line(Line::from_points(
+                        context.camera_matrix.ground_to_pixel(line.point).unwrap(),
+                        context
+                            .camera_matrix
+                            .ground_to_pixel(line.point + line.direction)
+                            .unwrap(),
+                    )),
+                    RansacFeature::TwoLines(two_lines) => {
+                        let point = context
+                            .camera_matrix
+                            .ground_to_pixel(two_lines.point)
+                            .unwrap();
+                        RansacFeature::TwoLines(TwoLines {
+                            point,
+                            direction1: context
+                                .camera_matrix
+                                .ground_to_pixel(two_lines.point + two_lines.direction1.normalize())
+                                .unwrap_or(point)
+                                - point,
+                            direction2: context
+                                .camera_matrix
+                                .ground_to_pixel(two_lines.point + two_lines.direction2.normalize())
+                                .unwrap_or(point)
+                                - point,
+                        })
+                    }
+                })
+                .collect()
+        });
         context.lines_in_image.fill_if_subscribed(|| {
             line_segments
                 .iter()

crates/vision/src/line_detection.rs

@@ -34,8 +34,10 @@ linear_algebra = { workspace = true }
 log = { workspace = true }
 mlua = { workspace = true }
 nalgebra = { workspace = true }
+ordered-float = { workspace = true }
 parameters = { workspace = true }
 projection = { workspace = true }
+ransac = { workspace = true }
 repository = { workspace = true }
 serde = { workspace = true }
 serde_json = { workspace = true }

tools/twix/Cargo.toml

@@ -3,6 +3,7 @@ use eframe::epaint::{Color32, Stroke};
 
 use coordinate_systems::Pixel;
 use geometry::line_segment::LineSegment;
+use ransac::RansacFeature;
 use types::{image_segments::GenericSegment, line_data::LineDiscardReason};
 
 use crate::{
@@ -14,6 +15,7 @@ use crate::{
 type DiscardedLines = Vec<(LineSegment<Pixel>, LineDiscardReason)>;
 
 pub struct LineDetection {
+    detected_features: BufferHandle<Option<Vec<RansacFeature<Pixel>>>>,
     lines_in_image: BufferHandle<Option<Vec<LineSegment<Pixel>>>>,
     discarded_lines: BufferHandle<Option<DiscardedLines>>,
     filtered_segments: BufferHandle<Option<Vec<GenericSegment>>>,
@@ -25,6 +27,9 @@ impl Overlay for LineDetection {
     fn new(nao: std::sync::Arc<crate::nao::Nao>, selected_cycler: VisionCycler) -> Self {
         let cycler_path = selected_cycler.as_path();
         Self {
+            detected_features: nao.subscribe_value(format!(
+                "{cycler_path}.additional_outputs.detected_features"
+            )),
             lines_in_image: nao
                 .subscribe_value(format!("{cycler_path}.additional_outputs.lines_in_image")),
             discarded_lines: nao
@@ -36,6 +41,9 @@ impl Overlay for LineDetection {
     }
 
     fn paint(&self, painter: &TwixPainter<Pixel>) -> Result<()> {
+        let Some(detected_features) = self.detected_features.get_last_value()?.flatten() else {
+            return Ok(());
+        };
         let Some(lines_in_image) = self.lines_in_image.get_last_value()?.flatten() else {
             return Ok(());
         };
@@ -45,6 +53,26 @@ impl Overlay for LineDetection {
         let Some(filtered_segments) = self.filtered_segments.get_last_value()?.flatten() else {
             return Ok(());
         };
+        for feature in detected_features {
+            match feature {
+                RansacFeature::None => {}
+                RansacFeature::Line(line) => {
+                    painter.line(line.point, line.direction, Stroke::new(2.0, Color32::RED))
+                }
+                RansacFeature::TwoLines(two_lines) => {
+                    painter.line(
+                        two_lines.point,
+                        two_lines.direction1,
+                        Stroke::new(2.0, Color32::GREEN),
+                    );
+                    painter.line(
+                        two_lines.point,
+                        two_lines.direction2,
+                        Stroke::new(2.0, Color32::GREEN),
+                    );
+                }
+            };
+        }
         for segment in filtered_segments {
             let stroke = Stroke::new(1.0, Color32::RED);
             painter.line_segment(segment.start.cast(), segment.end.cast(), stroke);

tools/twix/src/panels/image/overlays/line_detection.rs

@@ -286,6 +286,13 @@ impl<World> TwixPainter<World> {
         self.painter.line_segment([start, end], stroke);
     }
 
+    pub fn line(&self, point: Point2<World>, direction: Vector2<World>, stroke: Stroke) {
+        let start = self.transform_world_to_pixel(point - direction * 10000.0);
+        let end = self.transform_world_to_pixel(point + direction * 10000.0);
+        let stroke = self.transform_stroke(stroke);
+        self.painter.line_segment([start, end], stroke);
+    }
+
     pub fn rect_filled(&self, min: Point2<World>, max: Point2<World>, fill_color: Color32) {
         let right_bottom = point![max.x(), min.y()];
         let left_top = point![min.x(), max.y()];