[WIP] Polygon and Circle Detection (#100)

* Created FindPolygonPipe and DetectPolygonPipeline

* Return CVShape based off approxDP

* Added fromSides method to ContourShape

* Use enums

* Use harris detector

* Switch back to approxpolydp

* Added colourshape pipeline

* Added pipeline test

* Finsihed triangle/quad/custom polygon detection

* Circle detection

* Revert "Circle detection"

This reverts commit f6b2fe785d69b16ca1466a13073dce72a0d54570.

* Added shape drawings to draw2dcontourspipe

* Added circledetection pipe params

* apply spotless

* Added colourtoscalar outside of loop

* Added powercell testing images from ML library

* Powercell tracking works

* Added gradle to gitnore

* Added solvepnp to circles

* Reordered pipes and pipe params

* Fixed tests

* Apply spotless

* chmod gradlew

* gradle wrapper

* Removed commits from gradle

* Fix typo in ColoredShapePipeline

* Apply Spotless

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>

.gradle/buildOutputCleanup/cache.properties

@@ -0,0 +1,2 @@
+#Sun May 31 18:05:12 EDT 2020
+gradle.version=6.0.1

chameleon-server/gradlew

@@ -1,5 +1,21 @@
 #!/usr/bin/env sh
 
+#
+# Copyright 2015 the original author or authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
 ##############################################################################
 ##
 ##  Gradle start up script for UN*X
@@ -28,7 +44,7 @@ APP_NAME="Gradle"
 APP_BASE_NAME=`basename "$0"`
 
 # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-DEFAULT_JVM_OPTS='"-Xmx64m"'
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
 
 # Use the maximum available, or set MAX_FD != -1 to use that value.
 MAX_FD="maximum"
@@ -109,8 +125,8 @@ if $darwin; then
     GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
 fi
 
-# For Cygwin, switch paths to Windows format before running java
-if $cygwin ; then
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then
     APP_HOME=`cygpath --path --mixed "$APP_HOME"`
     CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
     JAVACMD=`cygpath --unix "$JAVACMD"`
@@ -138,19 +154,19 @@ if $cygwin ; then
         else
             eval `echo args$i`="\"$arg\""
         fi
-        i=$((i+1))
+        i=`expr $i + 1`
     done
     case $i in
-        (0) set -- ;;
-        (1) set -- "$args0" ;;
-        (2) set -- "$args0" "$args1" ;;
-        (3) set -- "$args0" "$args1" "$args2" ;;
-        (4) set -- "$args0" "$args1" "$args2" "$args3" ;;
-        (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
-        (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
-        (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
-        (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
-        (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
+        0) set -- ;;
+        1) set -- "$args0" ;;
+        2) set -- "$args0" "$args1" ;;
+        3) set -- "$args0" "$args1" "$args2" ;;
+        4) set -- "$args0" "$args1" "$args2" "$args3" ;;
+        5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
+        6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
+        7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
+        8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
+        9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
     esac
 fi
 
@@ -159,14 +175,9 @@ save () {
     for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done
     echo " "
 }
-APP_ARGS=$(save "$@")
+APP_ARGS=`save "$@"`
 
 # Collect all arguments for the java command, following the shell quoting and substitution rules
 eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS"
 
-# by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong
-if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then
-  cd "$(dirname "$0")"
-fi
-
 exec "$JAVACMD" "$@"

chameleon-server/gradlew.bat

@@ -1,3 +1,19 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
 @if "%DEBUG%" == "" @echo off
 @rem ##########################################################################
 @rem
@@ -14,7 +30,7 @@ set APP_BASE_NAME=%~n0
 set APP_HOME=%DIRNAME%
 
 @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-set DEFAULT_JVM_OPTS="-Xmx64m"
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
 
 @rem Find java.exe
 if defined JAVA_HOME goto findJavaFromJavaHome

chameleon-server/src/main/java/com/chameleonvision/common/util/TestUtils.java

@@ -77,6 +77,41 @@ public class TestUtils {
         }
     }
 
+    public enum PolygonTestImages {
+        kPolygons;
+
+        public final Path path;
+
+        Path getPath() {
+            var filename = this.toString().substring(1).toLowerCase();
+            return Path.of("polygons", filename + ".png");
+        }
+
+        PolygonTestImages() {
+            this.path = getPath();
+        }
+    }
+
+    public enum PowercellTestImages {
+        kPowercell_test_1,
+        kPowercell_test_2,
+        kPowercell_test_3,
+        kPowercell_test_4,
+        kPowercell_test_5,
+        kPowercell_test_6;
+
+        public final Path path;
+
+        Path getPath() {
+            var filename = this.toString().substring(1).toLowerCase();
+            return Path.of(filename + ".png");
+        }
+
+        PowercellTestImages() {
+            this.path = getPath();
+        }
+    }
+
     private static Path getResourcesFolderPath() {
         return Path.of("src", "test", "resources").toAbsolutePath();
     }
@@ -89,6 +124,10 @@ public class TestUtils {
         return getResourcesFolderPath().resolve("calibration");
     }
 
+    public static Path getPowercellPath() {
+        return getTestImagesPath().resolve("polygons").resolve("powercells");
+    }
+
     public static Path getWPIImagePath(WPI2020Image image) {
         return getTestImagesPath().resolve(image.path);
     }
@@ -97,6 +136,14 @@ public class TestUtils {
         return getTestImagesPath().resolve(image.path);
     }
 
+    public static Path getPolygonImagePath(PolygonTestImages image) {
+        return getTestImagesPath().resolve(image.path);
+    }
+
+    public static Path getPowercellImagePath(PowercellTestImages image) {
+        return getPowercellPath().resolve(image.path);
+    }
+
     public static void loadLibraries() {
         try {
             CameraServerCvJNI.forceLoad();

chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/FileFrameProvider.java

@@ -53,7 +53,8 @@ public class FileFrameProvider implements FrameProvider {
         Mat image = Imgcodecs.imread(m_path.toString());
 
         if (image.cols() > 0 && image.rows() > 0) {
-            FrameStaticProperties m_properties = new FrameStaticProperties(image.width(), image.height(), m_fov);
+            FrameStaticProperties m_properties =
+                    new FrameStaticProperties(image.width(), image.height(), m_fov);
             m_frame = new Frame(new CVMat(image), m_properties);
         } else {
             throw new RuntimeException("Image loading failed!");

chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVShape.java

@@ -23,6 +23,10 @@ public class CVShape {
         customTarget = targetPoints;
     }
 
+    public Contour getContour() {
+        return contour;
+    }
+
     public MatOfPoint2f getApproxPolyDp(double epsilon, boolean closed) {
         approxCurve.release();
         approxCurve = new MatOfPoint2f();

chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourShape.java

@@ -1,5 +1,8 @@
 package com.chameleonvision.common.vision.opencv;
 
+import java.util.EnumSet;
+import java.util.HashMap;
+
 public enum ContourShape {
     Custom(-1),
     Circle(0),
@@ -11,4 +14,16 @@ public enum ContourShape {
     ContourShape(int sides) {
         this.sides = sides;
     }
+
+    private static final HashMap<Integer, ContourShape> sidesToValueMap = new HashMap<>();
+
+    static {
+        for (var value : EnumSet.allOf(ContourShape.class)) {
+            sidesToValueMap.put(value.sides, value);
+        }
+    }
+
+    public static ContourShape fromSides(int sides) {
+        return sidesToValueMap.get(sides);
+    }
 }

chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw2dContoursPipe.java

@@ -7,11 +7,7 @@ import java.awt.Color;
 import java.util.ArrayList;
 import java.util.List;
 import org.apache.commons.lang3.tuple.Pair;
-import org.opencv.core.Mat;
-import org.opencv.core.MatOfPoint;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.RotatedRect;
+import org.opencv.core.*;
 import org.opencv.imgproc.Imgproc;
 
 public class Draw2dContoursPipe
@@ -22,7 +18,16 @@ public class Draw2dContoursPipe
     @Override
     protected Mat process(Pair<Mat, List<TrackedTarget>> in) {
         if (!in.getRight().isEmpty()
-                && (params.showCentroid || params.showMaximumBox || params.showRotatedBox)) {
+                && (params.showCentroid
+                        || params.showMaximumBox
+                        || params.showRotatedBox
+                        || params.showShape)) {
+
+            var centroidColour = ColorHelper.colorToScalar(params.centroidColor);
+            var maximumBoxColour = ColorHelper.colorToScalar(params.maximumBoxColor);
+            var rotatedBoxColour = ColorHelper.colorToScalar(params.rotatedBoxColor);
+            var shapeColour = ColorHelper.colorToScalar(params.shapeOutlineColour);
+
             for (int i = 0; i < (params.showMultiple ? in.getRight().size() : 1); i++) {
                 Point[] vertices = new Point[4];
                 MatOfPoint contour = new MatOfPoint();
@@ -46,11 +51,7 @@ public class Draw2dContoursPipe
 
                 if (params.showRotatedBox) {
                     Imgproc.drawContours(
-                            in.getLeft(),
-                            m_drawnContours,
-                            0,
-                            ColorHelper.colorToScalar(params.rotatedBoxColor),
-                            params.boxOutlineSize);
+                            in.getLeft(), m_drawnContours, 0, rotatedBoxColour, params.boxOutlineSize);
                 }
 
                 if (params.showMaximumBox) {
@@ -59,17 +60,21 @@ public class Draw2dContoursPipe
                             in.getLeft(),
                             new Point(box.x, box.y),
                             new Point(box.x + box.width, box.y + box.height),
-                            ColorHelper.colorToScalar(params.maximumBoxColor),
+                            maximumBoxColour,
+                            params.boxOutlineSize);
+                }
+
+                if (params.showShape) {
+                    Imgproc.drawContours(
+                            in.getLeft(),
+                            List.of(target.m_mainContour.mat),
+                            -1,
+                            shapeColour,
                             params.boxOutlineSize);
                 }
 
                 if (params.showCentroid) {
-                    Imgproc.circle(
-                            in.getLeft(),
-                            target.getTargetOffsetPoint(),
-                            3,
-                            ColorHelper.colorToScalar(params.centroidColor),
-                            2);
+                    Imgproc.circle(in.getLeft(), target.getTargetOffsetPoint(), 3, centroidColour, 2);
                 }
             }
         }
@@ -82,10 +87,12 @@ public class Draw2dContoursPipe
         public boolean showMultiple = true;
         public int boxOutlineSize = 1;
         public boolean showRotatedBox = true;
+        public boolean showShape = false;
         public boolean showMaximumBox = true;
         public Color centroidColor = Color.GREEN;
         public Color rotatedBoxColor = Color.BLUE;
         public Color maximumBoxColor = Color.RED;
+        public Color shapeOutlineColour = Color.MAGENTA;
 
         // TODO: set other params from UI/settings file?
         public Draw2dContoursParams(boolean showMultipleTargets) {

chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FilterShapesPipe.java

@@ -0,0 +1,44 @@
+package com.chameleonvision.common.vision.pipe.impl;
+
+import com.chameleonvision.common.vision.opencv.CVShape;
+import com.chameleonvision.common.vision.opencv.ContourShape;
+import com.chameleonvision.common.vision.pipe.CVPipe;
+import java.util.List;
+
+public class FilterShapesPipe
+        extends CVPipe<List<CVShape>, List<CVShape>, FilterShapesPipe.FilterShapesPipeParams> {
+    /**
+    * Runs the process for the pipe.
+    *
+    * @param in Input for pipe processing.
+    * @return Result of processing.
+    */
+    @Override
+    protected List<CVShape> process(List<CVShape> in) {
+        in.removeIf(
+                shape ->
+                        shape.shape != params.desiredShape
+                                || shape.contour.getArea() > params.maxArea
+                                || shape.contour.getArea() < params.minArea
+                                || shape.contour.getPerimeter() > params.maxPeri
+                                || shape.contour.getPerimeter() < params.minPeri);
+        return in;
+    }
+
+    public static class FilterShapesPipeParams {
+        ContourShape desiredShape;
+        double minArea;
+        double maxArea;
+        double minPeri;
+        double maxPeri;
+
+        public FilterShapesPipeParams(
+                ContourShape desiredShape, double minArea, double maxArea, double minPeri, double maxPeri) {
+            this.desiredShape = desiredShape;
+            this.minArea = minArea;
+            this.maxArea = maxArea;
+            this.minPeri = minPeri;
+            this.maxPeri = maxPeri;
+        }
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FindCirclesPipe.java

@@ -0,0 +1,82 @@
+package com.chameleonvision.common.vision.pipe.impl;
+
+import com.chameleonvision.common.vision.opencv.CVShape;
+import com.chameleonvision.common.vision.opencv.Contour;
+import com.chameleonvision.common.vision.opencv.ContourShape;
+import com.chameleonvision.common.vision.pipe.CVPipe;
+import java.util.ArrayList;
+import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.imgproc.Imgproc;
+import org.opencv.imgproc.Moments;
+
+public class FindCirclesPipe
+        extends CVPipe<Pair<Mat, List<Contour>>, List<CVShape>, FindCirclesPipe.FindCirclePipeParams> {
+
+    double[] c;
+    Mat circles = new Mat();
+    Moments mu;
+    double x_center;
+    double y_center;
+    /**
+    * Runs the process for the pipe.
+    *
+    * @param in Input for pipe processing.
+    * @return Result of processing.
+    */
+    @Override
+    protected List<CVShape> process(Pair<Mat, List<Contour>> in) {
+        circles.release();
+        List<CVShape> output = new ArrayList<>();
+
+        Imgproc.HoughCircles(
+                in.getLeft(),
+                circles,
+                Imgproc.HOUGH_GRADIENT,
+                1.0,
+                params.minDist,
+                params.maxCannyThresh,
+                params.accuracy,
+                params.minRadius,
+                params.maxRadius);
+        for (int x = 0; x < circles.cols(); x++) {
+            c = circles.get(0, x);
+            x_center = c[0];
+            y_center = c[1];
+            for (Contour contour : in.getRight()) {
+                mu = contour.getMoments();
+                if (Math.abs(x_center - (mu.m10 / mu.m00)) <= params.allowableThreshold
+                        && Math.abs(y_center - (mu.m01 / mu.m00)) <= params.allowableThreshold) {
+                    output.add(new CVShape(contour, ContourShape.Circle));
+                }
+            }
+        }
+
+        return output;
+    }
+
+    public static class FindCirclePipeParams {
+        public int allowableThreshold;
+        public int minRadius;
+        public int maxRadius;
+        public int minDist;
+        public int maxCannyThresh;
+        public int accuracy;
+
+        public FindCirclePipeParams(
+                int allowableThreshold,
+                int minRadius,
+                int minDist,
+                int maxRadius,
+                int maxCannyThresh,
+                int accuracy) {
+            this.allowableThreshold = allowableThreshold;
+            this.minRadius = minRadius;
+            this.maxRadius = maxRadius;
+            this.minDist = minDist;
+            this.maxCannyThresh = maxCannyThresh;
+            this.accuracy = accuracy;
+        }
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FindPolygonPipe.java

@@ -0,0 +1,68 @@
+package com.chameleonvision.common.vision.pipe.impl;
+
+import com.chameleonvision.common.vision.opencv.CVShape;
+import com.chameleonvision.common.vision.opencv.Contour;
+import com.chameleonvision.common.vision.opencv.ContourShape;
+import com.chameleonvision.common.vision.pipe.CVPipe;
+import java.util.ArrayList;
+import java.util.List;
+import org.opencv.core.*;
+import org.opencv.imgproc.Imgproc;
+
+public class FindPolygonPipe
+        extends CVPipe<List<Contour>, List<CVShape>, FindPolygonPipe.FindPolygonPipeParams> {
+    private int corners;
+    private MatOfPoint2f approx = new MatOfPoint2f();
+
+    /*
+    * Runs the process for the pipe.
+    *
+    * @param in Input for pipe processing.
+    * @return Result of processing.
+    */
+    @Override
+    protected List<CVShape> process(List<Contour> in) {
+        // List containing all the output shapes
+        List<CVShape> output = new ArrayList<>();
+
+        for (Contour contour : in) output.add(getShape(contour));
+
+        return output;
+    }
+
+    private CVShape getShape(Contour in) {
+
+        corners = getCorners(in);
+        if (ContourShape.fromSides(corners) == null) {
+            return new CVShape(in, ContourShape.Custom);
+        }
+        switch (ContourShape.fromSides(corners)) {
+            case Circle:
+                return new CVShape(in, ContourShape.Circle);
+            case Triangle:
+                return new CVShape(in, ContourShape.Triangle);
+            case Quadrilateral:
+                return new CVShape(in, ContourShape.Quadrilateral);
+        }
+
+        return new CVShape(in, ContourShape.Custom);
+    }
+
+    private int getCorners(Contour contour) {
+        approx.release();
+        Imgproc.approxPolyDP(
+                contour.getMat2f(),
+                approx,
+                params.accuracyPercentage / 600.0 * Imgproc.arcLength(contour.getMat2f(), true),
+                true);
+        return (int) approx.size().height;
+    }
+
+    public static class FindPolygonPipeParams {
+        double accuracyPercentage;
+
+        public FindPolygonPipeParams(double accuracyPercentage) {
+            this.accuracyPercentage = accuracyPercentage;
+        }
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipeline.java

@@ -1,16 +1,258 @@
 package com.chameleonvision.common.vision.pipeline;
 
+import static com.chameleonvision.common.vision.pipe.impl.FilterShapesPipe.*;
+import static com.chameleonvision.common.vision.pipe.impl.FindPolygonPipe.*;
+
+import com.chameleonvision.common.util.math.MathUtils;
 import com.chameleonvision.common.vision.frame.Frame;
 import com.chameleonvision.common.vision.frame.FrameStaticProperties;
+import com.chameleonvision.common.vision.opencv.*;
+import com.chameleonvision.common.vision.pipe.CVPipeResult;
+import com.chameleonvision.common.vision.pipe.impl.*;
+import com.chameleonvision.common.vision.target.PotentialTarget;
+import com.chameleonvision.common.vision.target.TrackedTarget;
+import java.util.Arrays;
+import java.util.List;
+import java.util.stream.Collectors;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
 
 public class ColoredShapePipeline
         extends CVPipeline<CVPipelineResult, ColoredShapePipelineSettings> {
+
+    private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
+    private final ErodeDilatePipe erodeDilatePipe = new ErodeDilatePipe();
+    private final HSVPipe hsvPipe = new HSVPipe();
+    private final OutputMatPipe outputMatPipe = new OutputMatPipe();
+    private final SpeckleRejectPipe speckleRejectPipe = new SpeckleRejectPipe();
+    private final FindContoursPipe findContoursPipe = new FindContoursPipe();
+    private final FindPolygonPipe findPolygonPipe = new FindPolygonPipe();
+    private final FindCirclesPipe findCirclesPipe = new FindCirclesPipe();
+    private final FilterShapesPipe filterShapesPipe = new FilterShapesPipe();
+    private final GroupContoursPipe groupContoursPipe = new GroupContoursPipe();
+    private final SortContoursPipe sortContoursPipe = new SortContoursPipe();
+    private final Collect2dTargetsPipe collect2dTargetsPipe = new Collect2dTargetsPipe();
+    private final CornerDetectionPipe cornerDetectionPipe = new CornerDetectionPipe();
+    private final SolvePNPPipe solvePNPPipe = new SolvePNPPipe();
+    private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
+    private final Draw2dContoursPipe draw2dContoursPipe = new Draw2dContoursPipe();
+    private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
+
+    private final Mat rawInputMat = new Mat();
+    private final DualMat outputMats = new DualMat();
+    private List<CVShape> shapes;
+    private CVPipeResult<Mat> result;
+    private CVPipeResult<List<TrackedTarget>> targetList;
+    private final Point[] rectPoints = new Point[4];
+
+    ColoredShapePipeline() {
+        settings = new ColoredShapePipelineSettings();
+    }
+
     @Override
     protected void setPipeParams(
-            FrameStaticProperties frameStaticProperties, ColoredShapePipelineSettings settings) {}
+            FrameStaticProperties frameStaticProperties, ColoredShapePipelineSettings settings) {
+
+        RotateImagePipe.RotateImageParams rotateImageParams =
+                new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
+        rotateImagePipe.setParams(rotateImageParams);
+
+        ErodeDilatePipe.ErodeDilateParams erodeDilateParams =
+                new ErodeDilatePipe.ErodeDilateParams(settings.erode, settings.dilate, 5);
+        // TODO: add kernel size to pipeline settings
+        erodeDilatePipe.setParams(erodeDilateParams);
+
+        HSVPipe.HSVParams hsvParams =
+                new HSVPipe.HSVParams(settings.hsvHue, settings.hsvSaturation, settings.hsvValue);
+        hsvPipe.setParams(hsvParams);
+
+        OutputMatPipe.OutputMatParams outputMatParams =
+                new OutputMatPipe.OutputMatParams(settings.outputShowThresholded);
+        outputMatPipe.setParams(outputMatParams);
+
+        SpeckleRejectPipe.SpeckleRejectParams speckleRejectParams =
+                new SpeckleRejectPipe.SpeckleRejectParams(settings.contourSpecklePercentage);
+        speckleRejectPipe.setParams(speckleRejectParams);
+
+        FindContoursPipe.FindContoursParams findContoursParams =
+                new FindContoursPipe.FindContoursParams();
+        findContoursPipe.setParams(findContoursParams);
+
+        FindPolygonPipeParams findPolygonPipeParams =
+                new FindPolygonPipeParams(settings.accuracyPercentage);
+        findPolygonPipe.setParams(findPolygonPipeParams);
+
+        FindCirclesPipe.FindCirclePipeParams findCirclePipeParams =
+                new FindCirclesPipe.FindCirclePipeParams(
+                        settings.allowableThreshold,
+                        settings.minRadius,
+                        settings.minDist,
+                        settings.maxRadius,
+                        settings.maxCannyThresh,
+                        settings.accuracy);
+        findCirclesPipe.setParams(findCirclePipeParams);
+
+        FilterShapesPipeParams filterShapesPipeParams =
+                new FilterShapesPipeParams(
+                        settings.desiredShape,
+                        settings.minArea,
+                        settings.maxArea,
+                        settings.minPeri,
+                        settings.maxPeri);
+        filterShapesPipe.setParams(filterShapesPipeParams);
+
+        GroupContoursPipe.GroupContoursParams groupContoursParams =
+                new GroupContoursPipe.GroupContoursParams(
+                        settings.contourGroupingMode, settings.contourIntersection);
+        groupContoursPipe.setParams(groupContoursParams);
+
+        SortContoursPipe.SortContoursParams sortContoursParams =
+                new SortContoursPipe.SortContoursParams(settings.contourSortMode, frameStaticProperties, 5);
+        sortContoursPipe.setParams(sortContoursParams);
+
+        Collect2dTargetsPipe.Collect2dTargetsParams collect2dTargetsParams =
+                new Collect2dTargetsPipe.Collect2dTargetsParams(
+                        frameStaticProperties,
+                        settings.offsetRobotOffsetMode,
+                        settings.offsetDualLineM,
+                        settings.offsetDualLineB,
+                        settings.offsetCalibrationPoint.toPoint(),
+                        settings.contourTargetOffsetPointEdge,
+                        settings.contourTargetOrientation);
+        collect2dTargetsPipe.setParams(collect2dTargetsParams);
+
+        var params =
+                new CornerDetectionPipe.CornerDetectionPipeParameters(
+                        settings.cornerDetectionStrategy,
+                        settings.cornerDetectionUseConvexHulls,
+                        settings.cornerDetectionExactSideCount,
+                        settings.cornerDetectionSideCount,
+                        settings.cornerDetectionAccuracyPercentage);
+        cornerDetectionPipe.setParams(params);
+
+        var solvePNPParams =
+                new SolvePNPPipe.SolvePNPPipeParams(
+                        settings.cameraCalibration, settings.cameraPitch, settings.targetModel);
+        solvePNPPipe.setParams(solvePNPParams);
+
+        Draw2dContoursPipe.Draw2dContoursParams draw2dContoursParams =
+                new Draw2dContoursPipe.Draw2dContoursParams(settings.outputShowMultipleTargets);
+        draw2dContoursParams.showShape = true;
+        draw2dContoursParams.showMaximumBox = false;
+        draw2dContoursParams.showRotatedBox = false;
+        draw2dContoursParams.boxOutlineSize = 2;
+        draw2dContoursPipe.setParams(draw2dContoursParams);
+
+        Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
+                new Draw2dCrosshairPipe.Draw2dCrosshairParams(
+                        settings.offsetRobotOffsetMode, settings.offsetCalibrationPoint);
+        draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
+
+        var draw3dContoursParams =
+                new Draw3dTargetsPipe.Draw3dContoursParams(
+                        settings.cameraCalibration, settings.targetModel);
+        draw3dTargetsPipe.setParams(draw3dContoursParams);
+    }
 
     @Override
     protected CVPipelineResult process(Frame frame, ColoredShapePipelineSettings settings) {
-        return null;
+        setPipeParams(frame.frameStaticProperties, settings);
+
+        long sumPipeNanosElapsed = 0L;
+
+        frame.image.getMat().copyTo(rawInputMat);
+
+        CVPipeResult<Mat> rotateImageResult = rotateImagePipe.apply(frame.image.getMat());
+        sumPipeNanosElapsed += rotateImageResult.nanosElapsed;
+
+        CVPipeResult<Mat> erodeDilateResult = erodeDilatePipe.apply(rotateImageResult.result);
+        sumPipeNanosElapsed += erodeDilateResult.nanosElapsed;
+
+        CVPipeResult<Mat> hsvPipeResult = hsvPipe.apply(erodeDilateResult.result);
+        sumPipeNanosElapsed += hsvPipeResult.nanosElapsed;
+
+        outputMats.first = rawInputMat;
+        outputMats.second = hsvPipeResult.result;
+
+        CVPipeResult<Mat> outputMatResult = outputMatPipe.apply(outputMats);
+        sumPipeNanosElapsed += outputMatResult.nanosElapsed;
+
+        CVPipeResult<List<Contour>> findContoursResult = findContoursPipe.apply(hsvPipeResult.result);
+        sumPipeNanosElapsed += findContoursResult.nanosElapsed;
+
+        CVPipeResult<List<Contour>> speckleRejectResult =
+                speckleRejectPipe.apply(findContoursResult.result);
+        sumPipeNanosElapsed += speckleRejectResult.nanosElapsed;
+
+        if (settings.desiredShape == ContourShape.Circle) {
+            CVPipeResult<List<CVShape>> findCirclesResult =
+                    findCirclesPipe.apply(Pair.of(hsvPipeResult.result, speckleRejectResult.result));
+            sumPipeNanosElapsed += findCirclesResult.nanosElapsed;
+            shapes = findCirclesResult.result;
+        } else {
+            CVPipeResult<List<CVShape>> findPolygonsResult =
+                    findPolygonPipe.apply(speckleRejectResult.result);
+            sumPipeNanosElapsed += findPolygonsResult.nanosElapsed;
+            shapes = findPolygonsResult.result;
+        }
+
+        CVPipeResult<List<CVShape>> filterShapeResult = filterShapesPipe.apply(shapes);
+        sumPipeNanosElapsed += filterShapeResult.nanosElapsed;
+
+        CVPipeResult<List<PotentialTarget>> groupContoursResult =
+                groupContoursPipe.apply(
+                        filterShapeResult.result.stream()
+                                .map(CVShape::getContour)
+                                .collect(Collectors.toList()));
+        sumPipeNanosElapsed += groupContoursResult.nanosElapsed;
+
+        CVPipeResult<List<PotentialTarget>> sortContoursResult =
+                sortContoursPipe.apply(groupContoursResult.result);
+        sumPipeNanosElapsed += sortContoursResult.nanosElapsed;
+
+        CVPipeResult<List<TrackedTarget>> collect2dTargetsResult =
+                collect2dTargetsPipe.apply(sortContoursResult.result);
+        sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed;
+
+        if (settings.solvePNPEnabled && settings.desiredShape == ContourShape.Circle) {
+            var cornerDetectionResult = cornerDetectionPipe.apply(collect2dTargetsResult.result);
+            collect2dTargetsResult.result.forEach(
+                    shape -> {
+                        shape.getMinAreaRect().points(rectPoints);
+                        shape.setCorners(Arrays.asList(rectPoints));
+                    });
+            sumPipeNanosElapsed += cornerDetectionResult.nanosElapsed;
+
+            var solvePNPResult = solvePNPPipe.apply(cornerDetectionResult.result);
+            sumPipeNanosElapsed += solvePNPResult.nanosElapsed;
+
+            targetList = solvePNPResult;
+        } else {
+            targetList = collect2dTargetsResult;
+        }
+
+        CVPipeResult<Mat> draw2dCrosshairResult =
+                draw2dCrosshairPipe.apply(Pair.of(outputMatResult.result, targetList.result));
+        sumPipeNanosElapsed += draw2dCrosshairResult.nanosElapsed;
+
+        CVPipeResult<Mat> draw2dContoursResult =
+                draw2dContoursPipe.apply(
+                        Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
+        sumPipeNanosElapsed += draw2dContoursResult.nanosElapsed;
+
+        if (settings.solvePNPEnabled && settings.desiredShape == ContourShape.Circle) {
+            result =
+                    draw3dTargetsPipe.apply(
+                            Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
+            sumPipeNanosElapsed += result.nanosElapsed;
+        } else {
+            result = draw2dContoursResult;
+        }
+
+        return new CVPipelineResult(
+                MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                collect2dTargetsResult.result,
+                new Frame(new CVMat(result.result), frame.frameStaticProperties));
     }
 }

chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipelineSettings.java

@@ -1,12 +1,49 @@
 package com.chameleonvision.common.vision.pipeline;
 
+import com.chameleonvision.common.calibration.CameraCalibrationCoefficients;
+import com.chameleonvision.common.vision.opencv.ContourGroupingMode;
+import com.chameleonvision.common.vision.opencv.ContourIntersectionDirection;
 import com.chameleonvision.common.vision.opencv.ContourShape;
+import com.chameleonvision.common.vision.pipe.impl.CornerDetectionPipe;
+import com.chameleonvision.common.vision.target.TargetModel;
 import com.fasterxml.jackson.annotation.JsonTypeName;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import java.util.Objects;
 
 @JsonTypeName("ColoredShapePipelineSettings")
 public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
-    ContourShape desiredShape;
+    public ContourShape desiredShape = ContourShape.Triangle;
+    public double minArea = Integer.MIN_VALUE;
+    public double maxArea = Integer.MAX_VALUE;
+    public double minPeri = Integer.MIN_VALUE;
+    public double maxPeri = Integer.MAX_VALUE;
+    public double accuracyPercentage = 10.0;
+    // Circle detection
+    public int allowableThreshold = 5;
+    public int minRadius = 0;
+    public int maxRadius = 0;
+    public int minDist = 10;
+    public int maxCannyThresh = 90;
+    public int accuracy = 20;
+    // how many contours to attempt to group (Single, Dual)
+    public ContourGroupingMode contourGroupingMode = ContourGroupingMode.Single;
+
+    // the direction in which contours must intersect to be considered intersecting
+    public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;
+
+    // 3d settings
+    public boolean solvePNPEnabled = false;
+    public CameraCalibrationCoefficients cameraCalibration;
+    public TargetModel targetModel;
+    public Rotation2d cameraPitch = Rotation2d.fromDegrees(0.0);
+
+    // Corner detection settings
+    public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
+            CornerDetectionPipe.DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS;
+    public boolean cornerDetectionUseConvexHulls = true;
+    public boolean cornerDetectionExactSideCount = false;
+    public int cornerDetectionSideCount = 4;
+    public double cornerDetectionAccuracyPercentage = 10;
 
     public ColoredShapePipelineSettings() {
         super();
@@ -19,11 +56,58 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
         if (o == null || getClass() != o.getClass()) return false;
         if (!super.equals(o)) return false;
         ColoredShapePipelineSettings that = (ColoredShapePipelineSettings) o;
-        return desiredShape == that.desiredShape;
+        return Double.compare(that.minArea, minArea) == 0
+                && Double.compare(that.maxArea, maxArea) == 0
+                && Double.compare(that.minPeri, minPeri) == 0
+                && Double.compare(that.maxPeri, maxPeri) == 0
+                && Double.compare(that.accuracyPercentage, accuracyPercentage) == 0
+                && allowableThreshold == that.allowableThreshold
+                && minRadius == that.minRadius
+                && maxRadius == that.maxRadius
+                && minDist == that.minDist
+                && maxCannyThresh == that.maxCannyThresh
+                && accuracy == that.accuracy
+                && solvePNPEnabled == that.solvePNPEnabled
+                && cornerDetectionUseConvexHulls == that.cornerDetectionUseConvexHulls
+                && cornerDetectionExactSideCount == that.cornerDetectionExactSideCount
+                && cornerDetectionSideCount == that.cornerDetectionSideCount
+                && Double.compare(that.cornerDetectionAccuracyPercentage, cornerDetectionAccuracyPercentage)
+                        == 0
+                && desiredShape == that.desiredShape
+                && contourGroupingMode == that.contourGroupingMode
+                && contourIntersection == that.contourIntersection
+                && Objects.equals(cameraCalibration, that.cameraCalibration)
+                && Objects.equals(targetModel, that.targetModel)
+                && Objects.equals(cameraPitch, that.cameraPitch)
+                && cornerDetectionStrategy == that.cornerDetectionStrategy;
     }
 
     @Override
     public int hashCode() {
-        return Objects.hash(super.hashCode(), desiredShape);
+        return Objects.hash(
+                super.hashCode(),
+                desiredShape,
+                minArea,
+                maxArea,
+                minPeri,
+                maxPeri,
+                accuracyPercentage,
+                allowableThreshold,
+                minRadius,
+                maxRadius,
+                minDist,
+                maxCannyThresh,
+                accuracy,
+                contourGroupingMode,
+                contourIntersection,
+                solvePNPEnabled,
+                cameraCalibration,
+                targetModel,
+                cameraPitch,
+                cornerDetectionStrategy,
+                cornerDetectionUseConvexHulls,
+                cornerDetectionExactSideCount,
+                cornerDetectionSideCount,
+                cornerDetectionAccuracyPercentage);
     }
 }

chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetModel.java

@@ -87,6 +87,16 @@ public class TargetModel implements Releasable {
         return new TargetModel(corners, 4);
     }
 
+    public static TargetModel getCircleTarget(double radius) {
+        var corners =
+                List.of(
+                        new Point3(-radius / 2, -radius / 2, -radius / 2),
+                        new Point3(-radius / 2, radius / 2, -radius / 2),
+                        new Point3(radius / 2, radius / 2, -radius / 2),
+                        new Point3(radius / 2, -radius / 2, -radius / 2));
+        return new TargetModel(corners, 0);
+    }
+
     @Override
     public boolean equals(Object o) {
         if (this == o) return true;

chameleon-server/src/test/java/com/chameleonvision/common/BenchmarkTest.java

@@ -115,7 +115,13 @@ public class BenchmarkTest {
         var frameProps = frameProvider.get().frameStaticProperties;
 
         // begin benchmark
-        System.out.println("Beginning " + secondsToRun + " second benchmark at resolution " + frameProps.imageWidth + "x" + frameProps.imageHeight);
+        System.out.println(
+                "Beginning "
+                        + secondsToRun
+                        + " second benchmark at resolution "
+                        + frameProps.imageWidth
+                        + "x"
+                        + frameProps.imageHeight);
         var benchmarkStartMillis = System.currentTimeMillis();
         do {
             CVPipelineResult pipelineResult = pipeline.run(frameProvider.get());

chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/CirclePNPTest.java

@@ -0,0 +1,164 @@
+package com.chameleonvision.common.vision.pipeline;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+
+import com.chameleonvision.common.calibration.CameraCalibrationCoefficients;
+import com.chameleonvision.common.util.TestUtils;
+import com.chameleonvision.common.vision.frame.Frame;
+import com.chameleonvision.common.vision.frame.provider.FileFrameProvider;
+import com.chameleonvision.common.vision.opencv.CVMat;
+import com.chameleonvision.common.vision.opencv.ContourGroupingMode;
+import com.chameleonvision.common.vision.opencv.ContourIntersectionDirection;
+import com.chameleonvision.common.vision.opencv.ContourShape;
+import com.chameleonvision.common.vision.target.TargetModel;
+import com.chameleonvision.common.vision.target.TrackedTarget;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.util.stream.Collectors;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+public class CirclePNPTest {
+
+    private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json";
+    private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json";
+
+    @BeforeEach
+    public void Init() {
+        TestUtils.loadLibraries();
+    }
+
+    @Test
+    public void loadCameraIntrinsics() {
+        var lifecam240pCal = getCoeffs(LIFECAM_240P_CAL_FILE);
+        var lifecam480pCal = getCoeffs(LIFECAM_480P_CAL_FILE);
+
+        assertNotNull(lifecam240pCal);
+        checkCameraCoefficients(lifecam240pCal);
+        assertNotNull(lifecam480pCal);
+        checkCameraCoefficients(lifecam480pCal);
+    }
+
+    private CameraCalibrationCoefficients getCoeffs(String filename) {
+        try {
+            var cameraCalibration =
+                    new ObjectMapper()
+                            .readValue(
+                                    (Path.of(TestUtils.getCalibrationPath().toString(), filename).toFile()),
+                                    CameraCalibrationCoefficients.class);
+
+            checkCameraCoefficients(cameraCalibration);
+
+            return cameraCalibration;
+        } catch (IOException e) {
+            e.printStackTrace();
+            return null;
+        }
+    }
+
+    private void checkCameraCoefficients(CameraCalibrationCoefficients cameraCalibration) {
+        assertEquals(3, cameraCalibration.cameraIntrinsics.rows);
+        assertEquals(3, cameraCalibration.cameraIntrinsics.cols);
+        assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows());
+        assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().cols());
+        assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows());
+        assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols());
+        assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().rows());
+        assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().cols());
+        assertEquals(1, cameraCalibration.cameraExtrinsics.rows);
+        assertEquals(5, cameraCalibration.cameraExtrinsics.cols);
+        assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMat().rows());
+        assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMat().cols());
+        assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().rows());
+        assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().cols());
+        assertEquals(1, cameraCalibration.getCameraExtrinsicsMat().rows());
+        assertEquals(5, cameraCalibration.getCameraExtrinsicsMat().cols());
+    }
+
+    @Test
+    public void testCircle() {
+        var pipeline = new ColoredShapePipeline();
+
+        pipeline.getSettings().hsvHue.set(0, 100);
+        pipeline.getSettings().hsvSaturation.set(100, 255);
+        pipeline.getSettings().hsvValue.set(100, 255);
+        pipeline.getSettings().outputShowThresholded = true;
+        pipeline.getSettings().maxCannyThresh = 50;
+        pipeline.getSettings().accuracy = 15;
+        pipeline.getSettings().allowableThreshold = 5;
+        pipeline.getSettings().solvePNPEnabled = true;
+        pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
+        pipeline.getSettings().cornerDetectionUseConvexHulls = true;
+        pipeline.getSettings().cameraCalibration = getCoeffs(LIFECAM_480P_CAL_FILE);
+        pipeline.getSettings().targetModel = TargetModel.getCircleTarget(7);
+        pipeline.getSettings().cameraPitch = Rotation2d.fromDegrees(0.0);
+        pipeline.getSettings().outputShowThresholded = true;
+        pipeline.getSettings().outputShowMultipleTargets = false;
+        pipeline.getSettings().contourGroupingMode = ContourGroupingMode.Single;
+        pipeline.getSettings().contourIntersection = ContourIntersectionDirection.Up;
+        pipeline.getSettings().desiredShape = ContourShape.Circle;
+        pipeline.getSettings().allowableThreshold = 10;
+        pipeline.getSettings().minRadius = 30;
+        pipeline.getSettings().accuracyPercentage = 30.0;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getPowercellImagePath(TestUtils.PowercellTestImages.kPowercell_test_6),
+                        TestUtils.WPI2020Image.FOV);
+
+        CVPipelineResult pipelineResult = pipeline.run(frameProvider.get());
+        printTestResults(pipelineResult);
+
+        TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999);
+    }
+
+    private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) {
+        var pipeline = new ReflectivePipeline();
+        pipeline.settings = settings;
+
+        while (true) {
+            CVPipelineResult pipelineResult = pipeline.run(frame);
+            printTestResults(pipelineResult);
+            int preRelease = CVMat.getMatCount();
+            pipelineResult.release();
+            int postRelease = CVMat.getMatCount();
+
+            System.out.printf("Pre: %d, Post: %d\n", preRelease, postRelease);
+        }
+    }
+
+    // used to run VisualVM for profiling, which won't run on unit tests.
+    public static void main(String[] args) {
+        TestUtils.loadLibraries();
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes),
+                        TestUtils.WPI2019Image.FOV);
+
+        var settings = new ReflectivePipelineSettings();
+        settings.hsvHue.set(60, 100);
+        settings.hsvSaturation.set(100, 255);
+        settings.hsvValue.set(190, 255);
+        settings.outputShowThresholded = true;
+        settings.outputShowMultipleTargets = true;
+        settings.contourGroupingMode = ContourGroupingMode.Dual;
+        settings.contourIntersection = ContourIntersectionDirection.Up;
+
+        continuouslyRunPipeline(frameProvider.get(), settings);
+    }
+
+    private static void printTestResults(CVPipelineResult pipelineResult) {
+        double fps = 1000 / pipelineResult.getLatencyMillis();
+        System.out.println(
+                "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)");
+        System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
+        System.out.println(
+                "Found targets at "
+                        + pipelineResult.targets.stream()
+                                .map(TrackedTarget::getRobotRelativePose)
+                                .collect(Collectors.toList()));
+    }
+}

chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipelineTest.java

@@ -0,0 +1,121 @@
+package com.chameleonvision.common.vision.pipeline;
+
+import com.chameleonvision.common.util.TestUtils;
+import com.chameleonvision.common.vision.frame.Frame;
+import com.chameleonvision.common.vision.frame.FrameStaticProperties;
+import com.chameleonvision.common.vision.frame.provider.FileFrameProvider;
+import com.chameleonvision.common.vision.opencv.ContourGroupingMode;
+import com.chameleonvision.common.vision.opencv.ContourIntersectionDirection;
+import com.chameleonvision.common.vision.opencv.ContourShape;
+import org.junit.jupiter.api.Test;
+
+public class ColoredShapePipelineTest {
+
+    public static void testTriangleDetection(
+            ColoredShapePipeline pipeline,
+            ColoredShapePipelineSettings settings,
+            FrameStaticProperties frameStaticProperties,
+            Frame frame) {
+        pipeline.setPipeParams(frameStaticProperties, settings);
+        CVPipelineResult colouredShapePipelineResult = pipeline.run(frame);
+        TestUtils.showImage(
+                colouredShapePipelineResult.outputFrame.image.getMat(), "Pipeline output: Triangle.");
+        printTestResults(colouredShapePipelineResult);
+    }
+
+    public static void testQuadrilateralDetection(
+            ColoredShapePipeline pipeline,
+            ColoredShapePipelineSettings settings,
+            FrameStaticProperties frameStaticProperties,
+            Frame frame) {
+        settings.desiredShape = ContourShape.Quadrilateral;
+        pipeline.setPipeParams(frameStaticProperties, settings);
+        CVPipelineResult colouredShapePipelineResult = pipeline.run(frame);
+        TestUtils.showImage(
+                colouredShapePipelineResult.outputFrame.image.getMat(), "Pipeline output: Quadrilateral.");
+        printTestResults(colouredShapePipelineResult);
+    }
+
+    public static void testCustomShapeDetection(
+            ColoredShapePipeline pipeline,
+            ColoredShapePipelineSettings settings,
+            FrameStaticProperties frameStaticProperties,
+            Frame frame) {
+        settings.desiredShape = ContourShape.Custom;
+        pipeline.setPipeParams(frameStaticProperties, settings);
+        CVPipelineResult colouredShapePipelineResult = pipeline.run(frame);
+        TestUtils.showImage(
+                colouredShapePipelineResult.outputFrame.image.getMat(), "Pipeline output: Custom.");
+        printTestResults(colouredShapePipelineResult);
+    }
+
+    @Test
+    public static void testCircleShapeDetection(
+            ColoredShapePipeline pipeline,
+            ColoredShapePipelineSettings settings,
+            FrameStaticProperties frameStaticProperties,
+            Frame frame) {
+        settings.desiredShape = ContourShape.Circle;
+        pipeline.setPipeParams(frameStaticProperties, settings);
+        CVPipelineResult colouredShapePipelineResult = pipeline.run(frame);
+        TestUtils.showImage(
+                colouredShapePipelineResult.outputFrame.image.getMat(), "Pipeline output: Circle.");
+        printTestResults(colouredShapePipelineResult);
+    }
+
+    @Test
+    public static void testPowercellDetection(
+            ColoredShapePipelineSettings settings, ColoredShapePipeline pipeline) {
+
+        settings.hsvHue.set(10, 40);
+        settings.hsvSaturation.set(100, 255);
+        settings.hsvValue.set(100, 255);
+        settings.maxCannyThresh = 50;
+        settings.accuracy = 15;
+        settings.allowableThreshold = 5;
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getPowercellImagePath(TestUtils.PowercellTestImages.kPowercell_test_6),
+                        TestUtils.WPI2019Image.FOV);
+        testCircleShapeDetection(
+                pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get());
+    }
+
+    public static void main(String[] args) {
+        TestUtils.loadLibraries();
+        System.out.println(TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center));
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getPolygonImagePath(TestUtils.PolygonTestImages.kPolygons),
+                        TestUtils.WPI2019Image.FOV);
+        var settings = new ColoredShapePipelineSettings();
+        settings.hsvHue.set(0, 100);
+        settings.hsvSaturation.set(100, 255);
+        settings.hsvValue.set(100, 255);
+        settings.outputShowThresholded = true;
+        settings.outputShowMultipleTargets = true;
+        settings.contourGroupingMode = ContourGroupingMode.Single;
+        settings.contourIntersection = ContourIntersectionDirection.Up;
+        settings.desiredShape = ContourShape.Triangle;
+        settings.allowableThreshold = 10;
+        settings.accuracyPercentage = 30.0;
+
+        ColoredShapePipeline pipeline = new ColoredShapePipeline();
+        testTriangleDetection(
+                pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get());
+        testQuadrilateralDetection(
+                pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get());
+        testCustomShapeDetection(
+                pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get());
+        testCircleShapeDetection(
+                pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get());
+        testPowercellDetection(settings, pipeline);
+    }
+
+    private static void printTestResults(CVPipelineResult pipelineResult) {
+        double fps = 1000 / pipelineResult.getLatencyMillis();
+        System.out.print(
+                "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " fps), ");
+        System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
+    }
+}