From 50fdfd8bce2fcc83de446bbb86394732ff267934 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Mon, 28 Feb 2022 00:44:22 -0500
Subject: [PATCH] Add outlier rejection (#432)

Uses standard deviations from mean x/y location to reject outliers
---
 photon-client/src/store/index.js              |  2 +
 .../src/views/PipelineViews/ContoursTab.vue   | 41 +++++++++++-
 .../common/util/math/MathUtils.java           | 58 ++++++++++++++--
 .../vision/pipe/impl/FilterContoursPipe.java  | 66 ++++++++++++++++++-
 .../vision/pipeline/ReflectivePipeline.java   |  4 +-
 .../pipeline/ReflectivePipelineSettings.java  |  3 +
 6 files changed, 167 insertions(+), 7 deletions(-)

diff --git a/photon-client/src/store/index.js b/photon-client/src/store/index.js
index 714534c57..9a462a262 100644
--- a/photon-client/src/store/index.js
+++ b/photon-client/src/store/index.js
@@ -69,6 +69,8 @@ export default new Vuex.Store({
                     contourRatio: [0, 12],
                     contourFullness: [0, 12],
                     contourSpecklePercentage: 5,
+                    contourFilterRangeX: 5,
+                    contourFilterRangeY: 5,
                     contourGroupingMode: 0,
                     contourIntersection: 0,
                     contourSortMode: 0,
diff --git a/photon-client/src/views/PipelineViews/ContoursTab.vue b/photon-client/src/views/PipelineViews/ContoursTab.vue
index c239e04ee..364bd6968 100644
--- a/photon-client/src/views/PipelineViews/ContoursTab.vue
+++ b/photon-client/src/views/PipelineViews/ContoursTab.vue
@@ -5,7 +5,7 @@
       name="Area"
       min="0"
       max="100"
-      step="0.1"
+      step="0.01"
       @input="handlePipelineData('contourArea')"
     />
     <CVrangeSlider
@@ -46,6 +46,26 @@
       @input="handlePipelineData('contourSpecklePercentage')"
     />
     <template v-if="currentPipelineType() !== 3">
+      <CVslider
+        v-model="contourFilterRangeX"
+        name="X filter tightness"
+        tooltip="Rejects contours whose center X is further than X standard deviations above/below the mean X location"
+        min="0.1"
+        max="4"
+        step="0.1"
+        :slider-cols="largeBox"
+        @input="handlePipelineData('contourFilterRangeX')"
+      />
+      <CVslider
+        v-model="contourFilterRangeY"
+        name="Y filter tightness"
+        tooltip="Rejects contours whose center Y is further than X standard deviations above/below the mean Y location"
+        min="0.1"
+        max="4"
+        step="0.1"
+        :slider-cols="largeBox"
+        @input="handlePipelineData('contourFilterRangeY')"
+      />
       <CVselect
         v-model="contourGroupingMode"
         name="Target Grouping"
@@ -207,6 +227,25 @@ export default {
         this.$store.commit("mutatePipeline", {"contourSpecklePercentage": val});
       }
     },
+    contourFilterRangeX: {
+      get() {
+        console.log(this.$store.getters.currentPipelineSettings.contourFilterRangeX)
+        return this.$store.getters.currentPipelineSettings.contourFilterRangeX
+      },
+      set(val) {
+        console.log("set")
+        console.log(val)
+        this.$store.commit("mutatePipeline", {"contourFilterRangeX": val});
+      }
+    },
+    contourFilterRangeY: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.contourFilterRangeY
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"contourFilterRangeY": val});
+      }
+    },
     contourGroupingMode: {
       get() {
         return this.$store.getters.currentPipelineSettings.contourGroupingMode
diff --git a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
index 4f65301c1..99afc4fce 100644
--- a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
+++ b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
@@ -18,6 +18,8 @@
 package org.photonvision.common.util.math;
 
 import edu.wpi.first.util.WPIUtilJNI;
+import java.util.Arrays;
+import java.util.List;
 
 public class MathUtils {
     MathUtils() {}
@@ -64,6 +66,58 @@ public class MathUtils {
         return (int) Math.floor(map((double) value, inMin, inMax, outMin, outMax) + 0.5);
     }
 
+    public static long wpiNanoTime() {
+        return microsToNanos(WPIUtilJNI.now());
+    }
+
+    /**
+     * Get the value of the nTh percentile of a list
+     *
+     * @param list The list to evaluate
+     * @param p The percentile, in [0,100]
+     * @return
+     */
+    public static double getPercentile(List<Double> list, double p) {
+        if ((p > 100) || (p <= 0)) {
+            throw new IllegalArgumentException("invalid quantile value: " + p);
+        }
+
+        if (list.size() == 0) {
+            return Double.NaN;
+        }
+        if (list.size() == 1) {
+            return list.get(0); // always return single value for n = 1
+        }
+
+        // Sort array.  We avoid a third copy here by just creating the
+        // list directly.
+        double[] sorted = new double[list.size()];
+        for (int i = 0; i < list.size(); i++) {
+            sorted[i] = list.get(i);
+        }
+        Arrays.sort(sorted);
+
+        return evaluateSorted(sorted, p);
+    }
+
+    private static double evaluateSorted(final double[] sorted, final double p) {
+        double n = sorted.length;
+        double pos = p * (n + 1) / 100;
+        double fpos = Math.floor(pos);
+        int intPos = (int) fpos;
+        double dif = pos - fpos;
+
+        if (pos < 1) {
+            return sorted[0];
+        }
+        if (pos >= n) {
+            return sorted[sorted.length - 1];
+        }
+        double lower = sorted[intPos - 1];
+        double upper = sorted[intPos];
+        return lower + dif * (upper - lower);
+    }
+
     /**
      * Linearly interpolates between two values.
      *
@@ -76,8 +130,4 @@ public class MathUtils {
     public static double lerp(double startValue, double endValue, double t) {
         return startValue + (endValue - startValue) * t;
     }
-
-    public static long wpiNanoTime() {
-        return microsToNanos(WPIUtilJNI.now());
-    }
 }
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FilterContoursPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FilterContoursPipe.java
index 90d0eac6d..19409c9d6 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FilterContoursPipe.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FilterContoursPipe.java
@@ -18,9 +18,12 @@
 package org.photonvision.vision.pipe.impl;
 
 import java.util.ArrayList;
+import java.util.Comparator;
 import java.util.List;
+import java.util.stream.Collectors;
 import org.opencv.core.Rect;
 import org.opencv.core.RotatedRect;
+import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.common.util.numbers.DoubleCouple;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.Contour;
@@ -36,9 +39,64 @@ public class FilterContoursPipe
         for (Contour contour : in) {
             filterContour(contour);
         }
+
+        // we need the whole list for outlier rejection
+        rejectOutliers(m_filteredContours, params.xTol, params.yTol);
+
         return m_filteredContours;
     }
 
+    private void rejectOutliers(List<Contour> list, double xTol, double yTol) {
+        if (list.size() < 2) return; // Must have at least 2 points to reject outliers
+
+/*
+        // Sort by X and find median
+        list.sort(Comparator.comparingDouble(c -> c.getCenterPoint().x));
+
+        double medianX = list.get(list.size() / 2).getCenterPoint().x;
+        if (list.size() % 2 == 0)
+            medianX = (medianX + list.get(list.size() / 2 - 1).getCenterPoint().x) / 2;
+*/
+
+        double meanX = list.stream().mapToDouble(it -> it.getCenterPoint().x).sum() / list.size();
+
+        double stdDevX =
+                list.stream().mapToDouble(it -> Math.pow(it.getCenterPoint().x - meanX, 2.0)).sum();
+        stdDevX /= (list.size() - 1);
+        stdDevX = Math.sqrt(stdDevX);
+
+/*
+        // Sort by Y and find median
+        list.sort(Comparator.comparingDouble(c -> c.getCenterPoint().y));
+
+        double medianY = list.get(list.size() / 2).getCenterPoint().y;
+        if (list.size() % 2 == 0)
+            medianY = (medianY + list.get(list.size() / 2 - 1).getCenterPoint().y) / 2;
+*/
+
+        double meanY = list.stream().mapToDouble(it -> it.getCenterPoint().y).sum() / list.size();
+
+        double stdDevY =
+                list.stream().mapToDouble(it -> Math.pow(it.getCenterPoint().y - meanY, 2.0)).sum();
+        stdDevY /= (list.size() - 1);
+        stdDevY = Math.sqrt(stdDevY);
+
+        for (var it = list.iterator(); it.hasNext(); ) {
+            // Reject points more than N standard devs above/below median
+            // That is, |point - median| > std dev * tol
+            Contour c = it.next();
+            double x = c.getCenterPoint().x;
+            double y = c.getCenterPoint().y;
+
+            if (Math.abs(x - meanX) > stdDevX * xTol) {
+                it.remove();
+            } else if (Math.abs(y - meanY) > stdDevY * yTol) {
+                it.remove();
+            }
+            // Otherwise we're good! Keep it in
+        }
+    }
+
     private void filterContour(Contour contour) {
         RotatedRect minAreaRect = contour.getMinAreaRect();
 
@@ -69,16 +127,22 @@ public class FilterContoursPipe
         private final DoubleCouple m_ratio;
         private final DoubleCouple m_fullness;
         private final FrameStaticProperties m_frameStaticProperties;
+        private final double xTol; // IQR tolerance for x
+        private final double yTol; // IQR tolerance for x
 
         public FilterContoursParams(
                 DoubleCouple area,
                 DoubleCouple ratio,
                 DoubleCouple extent,
-                FrameStaticProperties camProperties) {
+                FrameStaticProperties camProperties,
+                double xTol,
+                double yTol) {
             this.m_area = area;
             this.m_ratio = ratio;
             this.m_fullness = extent;
             this.m_frameStaticProperties = camProperties;
+            this.xTol = xTol;
+            this.yTol = yTol;
         }
 
         public DoubleCouple getArea() {
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java
index 13c375a7d..ac7476bed 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java
@@ -100,7 +100,9 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
                         settings.contourArea,
                         settings.contourRatio,
                         settings.contourFullness,
-                        frameStaticProperties);
+                        frameStaticProperties,
+                        settings.contourFilterRangeX,
+                        settings.contourFilterRangeY);
         filterContoursPipe.setParams(filterContoursParams);
 
         var groupContoursParams =
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java
index 84028b81e..7376f3962 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java
@@ -21,6 +21,9 @@ import com.fasterxml.jackson.annotation.JsonTypeName;
 
 @JsonTypeName("ReflectivePipelineSettings")
 public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
+    public double contourFilterRangeX = 2;
+    public double contourFilterRangeY = 2;
+
     public ReflectivePipelineSettings() {
         super();
         pipelineType = PipelineType.Reflective;