Add parameter to enable/disable fitting two lines

HULKs · Dec 18, 2024 · a2c9d72 · a2c9d72
1 parent 1a86cea
commit a2c9d72
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 14 deletions.
diff --git a/crates/ransac/src/lib.rs b/crates/ransac/src/lib.rs
@@ -159,6 +159,7 @@ impl<Frame> Ransac<Frame> {
         &mut self,
         random_number_generator: &mut impl Rng,
         iterations: usize,
+        fit_two_lines: bool,
         maximum_score_distance: f32,
         maximum_inclusion_distance: f32,
     ) -> RansacResult<Frame> {
@@ -184,14 +185,27 @@ impl<Frame> Ransac<Frame> {
 
         let best_feature = (0..iterations)
             .map(|_| {
-                let mut points = self
-                    .unused_points
-                    .choose_multiple(random_number_generator, 3);
-                let feature = RansacFeature::from_points(
-                    *points.next().unwrap(),
-                    *points.next().unwrap(),
-                    *points.next().unwrap(),
-                );
+                let feature = if fit_two_lines {
+                    let mut points = self
+                        .unused_points
+                        .choose_multiple(random_number_generator, 3);
+
+                    RansacFeature::from_points(
+                        *points.next().unwrap(),
+                        *points.next().unwrap(),
+                        *points.next().unwrap(),
+                    )
+                } else {
+                    let mut points = self
+                        .unused_points
+                        .choose_multiple(random_number_generator, 2);
+
+                    RansacFeature::Line(Line::from_points(
+                        *points.next().unwrap(),
+                        *points.next().unwrap(),
+                    ))
+                };
+
                 let score = feature.score(
                     self.unused_points.iter(),
                     maximum_score_distance,
@@ -232,7 +246,7 @@ mod test {
         let mut ransac = Ransac::<SomeFrame>::new(vec![]);
         let mut rng = ChaChaRng::from_entropy();
         assert_eq!(
-            ransac.next_feature(&mut rng, 10, 5.0, 5.0),
+            ransac.next_feature(&mut rng, 10, false, 5.0, 5.0),
             RansacResult::default()
         );
     }
@@ -242,7 +256,7 @@ mod test {
         let mut ransac = Ransac::<SomeFrame>::new(vec![]);
         let mut rng = ChaChaRng::from_entropy();
         assert_eq!(
-            ransac.next_feature(&mut rng, 10, 5.0, 5.0),
+            ransac.next_feature(&mut rng, 10, false, 5.0, 5.0),
             RansacResult::default()
         );
     }
@@ -256,7 +270,7 @@ mod test {
         let RansacResult {
             feature,
             used_points,
-        } = ransac.next_feature(&mut rng, 10, 5.0, 5.0);
+        } = ransac.next_feature(&mut rng, 10, false, 5.0, 5.0);
         println!("{feature:#?}");
         println!("{used_points:#?}");
 
@@ -282,7 +296,7 @@ mod test {
 
         let mut ransac = Ransac::<SomeFrame>::new(points.clone());
         let mut rng = ChaChaRng::from_entropy();
-        let result = ransac.next_feature(&mut rng, 15, 1.0, 1.0);
+        let result = ransac.next_feature(&mut rng, 15, false, 1.0, 1.0);
 
         if let RansacFeature::Line(line) = result.feature {
             assert_relative_eq!(line.slope(), slope, epsilon = 0.0001);

diff --git a/crates/vision/src/field_border_detection.rs b/crates/vision/src/field_border_detection.rs
@@ -113,6 +113,7 @@ fn find_border_lines(
     let result = ransac.next_feature(
         random_state,
         20,
+        false,
         first_line_association_distance,
         first_line_association_distance,
     );
@@ -126,6 +127,7 @@ fn find_border_lines(
     let result = ransac.next_feature(
         random_state,
         20,
+        false,
         second_line_association_distance,
         second_line_association_distance,
     );

diff --git a/crates/vision/src/line_detection.rs b/crates/vision/src/line_detection.rs
@@ -75,6 +75,7 @@ pub struct CycleContext {
     minimum_number_of_points_on_line:
         Parameter<usize, "line_detection.$cycler_instance.minimum_number_of_points_on_line">,
     ransac_iterations: Parameter<usize, "line_detection.$cycler_instance.ransac_iterations">,
+    ransac_fit_two_lines: Parameter<bool, "line_detection.$cycler_instance.ransac_fit_two_lines">,
 
     camera_matrix: RequiredInput<Option<CameraMatrix>, "camera_matrix?">,
     filtered_segments: Input<FilteredSegments, "filtered_segments">,
@@ -173,6 +174,7 @@ impl LineDetection {
             let ransac_result = ransac.next_feature(
                 &mut self.random_state,
                 *context.ransac_iterations,
+                *context.ransac_fit_two_lines,
                 *context.maximum_fit_distance_in_ground,
                 *context.maximum_fit_distance_in_ground + *context.margin_for_point_inclusion,
             );

diff --git a/etc/parameters/default.json b/etc/parameters/default.json
@@ -155,7 +155,8 @@
         "end": 0.1
       },
       "minimum_number_of_points_on_line": 10,
-      "ransac_iterations": 50
+      "ransac_iterations": 50,
+      "ransac_fit_two_lines": false
     },
     "vision_bottom": {
       "use_horizontal_segments": true,
@@ -182,7 +183,8 @@
         "end": 0.1
       },
       "minimum_number_of_points_on_line": 10,
-      "ransac_iterations": 50
+      "ransac_iterations": 50,
+      "ransac_fit_two_lines": false
     }
   },
   "field_border_detection": {