Filter Feet-Cluster-Standard-Deviation

crates/filtering/src/mean_clustering.rs

@@ -1,19 +1,35 @@
 use coordinate_systems::Ground;
-use linear_algebra::Point2;
+use linear_algebra::{IntoFramed, Point2, Vector2};
 use types::detected_feet::CountedCluster;
 
 pub trait MeanClustering {
     fn push(&mut self, other: Point2<Ground>);
+    fn mean(&self) -> Point2<Ground>;
+    fn standard_deviation(&self) -> Vector2<Ground>;
 }
 
 impl MeanClustering for CountedCluster {
     fn push(&mut self, other: Point2<Ground>) {
-        self.mean = (self.mean * self.samples as f32 + other.coords()) / (self.samples + 1) as f32;
+        self.sum += other.coords();
+        self.sum_squared += other.map(|x| x * x).coords();
         self.samples += 1;
+
         if other.x() < self.leftmost_point.x() {
             self.leftmost_point = other;
         } else if other.x() > self.rightmost_point.x() {
             self.rightmost_point = other;
         }
     }
+
+    fn mean(&self) -> Point2<Ground> {
+        self.sum / self.samples as f32
+    }
+
+    #[doc = "Computes the individual standard deviations of the x and y components of the cluster."]
+    fn standard_deviation(&self) -> Vector2<Ground> {
+        ((self.sum_squared / self.samples as f32) - (self.sum / self.samples as f32).map(|x| x * x))
+            .inner
+            .map(|x| x.sqrt())
+            .framed()
+    }
 }

crates/types/src/detected_feet.rs

@@ -1,4 +1,4 @@
-use linear_algebra::Point2;
+use linear_algebra::{Point2, Vector2};
 use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 
@@ -23,8 +23,9 @@ pub struct ClusterPoint {
     Default, Clone, Debug, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
 )]
 pub struct CountedCluster {
-    pub mean: Point2<Ground>,
     pub samples: usize,
     pub leftmost_point: Point2<Ground>,
     pub rightmost_point: Point2<Ground>,
+    pub sum: Vector2<Ground>,
+    pub sum_squared: Vector2<Ground>,
 }

crates/vision/src/feet_detection.rs

@@ -1,4 +1,5 @@
 use color_eyre::Result;
+use coordinate_systems::Ground;
 use itertools::Itertools;
 use serde::{Deserialize, Serialize};
 
@@ -8,8 +9,8 @@ use filtering::{
     statistics::{mean, standard_deviation},
 };
 use framework::{AdditionalOutput, MainOutput};
-use linear_algebra::distance;
 use linear_algebra::point;
+use linear_algebra::{distance, Vector2};
 use projection::{camera_matrix::CameraMatrix, Projection};
 use types::{
     ball_detection::BallPercept,
@@ -42,6 +43,8 @@ pub struct CycleContext {
     minimum_feet_width: Parameter<f32, "feet_detection.$cycler_instance.minimum_feet_width">,
     minimum_segment_height:
         Parameter<f32, "feet_detection.$cycler_instance.minimum_segment_height">,
+    maximum_standard_deviation:
+        Parameter<Vector2<Ground>, "feet_detection.$cycler_instance.maximum_standard_deviation">,
 
     balls: RequiredInput<Option<Vec<BallPercept>>, "balls?">,
     camera_matrix: RequiredInput<Option<CameraMatrix>, "camera_matrix?">,
@@ -83,6 +86,11 @@ impl FeetDetection {
         let clusters_in_ground: Vec<_> = clusters_in_ground
             .into_iter()
             .filter(|cluster| cluster.samples > *context.minimum_samples_per_cluster)
+            .filter(|cluster| {
+                let standard_deviation = cluster.standard_deviation();
+                standard_deviation.x() < context.maximum_standard_deviation.x()
+                    && standard_deviation.y() < context.maximum_standard_deviation.y()
+            })
             .filter(|cluster| {
                 cluster.rightmost_point.y() - cluster.leftmost_point.y()
                     >= *context.minimum_feet_width
@@ -94,7 +102,7 @@ impl FeetDetection {
 
         let positions = clusters_in_ground
             .into_iter()
-            .map(|cluster| cluster.mean)
+            .map(|cluster| cluster.mean())
             .collect();
         Ok(MainOutputs {
             detected_feet: DetectedFeet { positions }.into(),
@@ -211,7 +219,7 @@ fn cluster_scored_cluster_points(
         let nearest_cluster = clusters
             .iter_mut()
             .filter_map(|cluster| {
-                let distance = distance(cluster.mean, point.position_in_ground);
+                let distance = distance(cluster.mean(), point.position_in_ground);
                 if distance < maximum_cluster_distance {
                     Some((cluster, distance))
                 } else {
@@ -224,10 +232,11 @@ fn cluster_scored_cluster_points(
         match nearest_cluster {
             Some((cluster, _)) => cluster.push(point.position_in_ground),
             None => clusters.push(CountedCluster {
-                mean: point.position_in_ground,
                 samples: 1,
                 leftmost_point: point.position_in_ground,
                 rightmost_point: point.position_in_ground,
+                sum: point.position_in_ground,
+                sum_squared: point.position_in_ground.map(|x| x * x),
             }),
         }
     }

etc/parameters/default.json

@@ -19,7 +19,8 @@
       "minimum_luminance_standard_deviation": 5,
       "minimum_samples_per_cluster": 5,
       "minimum_feet_width": 0.0,
-      "minimum_segment_height": 0.08
+      "minimum_segment_height": 0.08,
+      "maximum_standard_deviation": [0.1, 0.1]
     },
     "vision_bottom": {
       "enable": false,
@@ -28,7 +29,8 @@
       "minimum_luminance_standard_deviation": 7,
       "minimum_samples_per_cluster": 8,
       "minimum_feet_width": 0.06,
-      "minimum_segment_height": 0.1
+      "minimum_segment_height": 0.1,
+      "maximum_standard_deviation": [0.1, 0.1]
     }
   },
   "whistle_detection": {

tools/twix/Cargo.toml

@@ -23,6 +23,7 @@ egui_dock = { workspace = true }
 egui_extras = { workspace = true }
 egui_plot = { workspace = true }
 fern = { workspace = true }
+filtering = { workspace = true }
 framework = { workspace = true }
 fuzzy-matcher = { workspace = true }
 geometry = { workspace = true }

tools/twix/src/panels/map/layers/feet_detection.rs

@@ -2,6 +2,7 @@ use std::sync::Arc;
 
 use color_eyre::Result;
 use eframe::epaint::Color32;
+use filtering::mean_clustering::MeanClustering;
 
 use coordinate_systems::Ground;
 use types::{
@@ -48,13 +49,13 @@ impl Layer<Ground> for FeetDetection {
         if let Some(clusters) = self.cluster_bottom.get_last_value()?.flatten() {
             for cluster in clusters {
                 let radius = 0.1;
-                painter.circle_filled(cluster.mean, radius, Color32::YELLOW);
+                painter.circle_filled(cluster.mean(), radius, Color32::YELLOW);
             }
         }
         if let Some(clusters) = self.cluster_top.get_last_value()?.flatten() {
             for cluster in clusters {
                 let radius = 0.1;
-                painter.circle_filled(cluster.mean, radius, Color32::YELLOW);
+                painter.circle_filled(cluster.mean(), radius, Color32::YELLOW);
             }
         }