Recreated all pipeline steps as Pipes and set up CVPipeline2d

Main/src/main/java/com/chameleonvision/classabstraction/camera/CameraProperties.java

@@ -31,9 +31,8 @@ public class CameraProperties {
     public CameraProperties(UsbCamera baseCamera, double fov) {
         FOV = fov;
 
+        // TODO: determine how to set the initial videomode properly
         videoModes = filterVideoModes(baseCamera.enumerateVideoModes());
-
-
     }
 
     private List<VideoMode> filterVideoModes(VideoMode[] videoModes) {

Main/src/main/java/com/chameleonvision/classabstraction/camera/CameraStaticProperties.java

@@ -9,8 +9,8 @@ public class CameraStaticProperties {
     public final int ImageHeight;
     public final double FOV;
     public final double ImageArea;
-    public final double CenterX;
-    public final double CenterY;
+    public final double centerX;
+    public final double centerY;
     public final double HorizontalFocalLength;
     public final double VerticalFocalLength;
 
@@ -19,8 +19,8 @@ public class CameraStaticProperties {
         ImageHeight = imageHeight;
         FOV = fov;
         ImageArea = ImageWidth * ImageHeight;
-        CenterX = ((double) ImageWidth / 2) - 0.5;
-        CenterY = ((double) ImageHeight / 2) - 0.5;
+        centerX = ((double) ImageWidth / 2) - 0.5;
+        centerY = ((double) ImageHeight / 2) - 0.5;
 
         // pinhole model calculations
         double diagonalView = FastMath.toRadians(FOV);

Main/src/main/java/com/chameleonvision/classabstraction/camera/USBCamera.java

@@ -5,7 +5,7 @@ import edu.wpi.cscore.VideoMode;
 
 public class USBCamera {
     private final UsbCamera baseCamera;
-    private final CameraProperties properties;
+    public final CameraProperties properties;
 
     public USBCamera(UsbCamera camera) {
         baseCamera = camera;

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline.java

@@ -1,21 +1,21 @@
 package com.chameleonvision.classabstraction.pipeline;
 
+import com.chameleonvision.classabstraction.camera.USBCamera;
 import org.opencv.core.Mat;
 
 /**
  *
  * @param <R> Pipeline result type
  */
-public abstract class CVPipeline<R> {
-    protected CVPipelineSettings settings;
+public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings> {
+    protected S settings;
     private Mat inputMat;
     protected Mat outputMat;
 
-    public CVPipeline(CVPipelineSettings settings) {
+    public CVPipeline(S settings) {
         this.settings = settings;
     }
 
-    abstract void initPipeline();
+    abstract void initPipeline(USBCamera camera);
     abstract R runPipeline(Mat inputMat);
-    abstract Mat getOutputMat();
 }

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline2d.java

@@ -1,191 +1,126 @@
 package com.chameleonvision.classabstraction.pipeline;
 
-import com.chameleonvision.util.MathHandler;
-import com.chameleonvision.vision.ImageFlipMode;
-import com.chameleonvision.vision.camera.CameraValues;
-import org.jetbrains.annotations.NotNull;
+import com.chameleonvision.classabstraction.camera.CameraStaticProperties;
+import com.chameleonvision.classabstraction.camera.USBCamera;
+import com.chameleonvision.classabstraction.pipeline.pipes.*;
+import com.chameleonvision.vision.ImageRotation;
+import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.*;
 import org.opencv.imgproc.Imgproc;
 
-import java.util.ArrayList;
 import java.util.List;
 
 @SuppressWarnings("WeakerAccess")
-public class CVPipeline2d extends CVPipeline<CVPipeline2d.CVPipeline2dResult> {
+public class CVPipeline2d extends CVPipeline<CVPipeline2d.CVPipeline2dResult, CVPipeline2d.CVPipeline2dSettings> {
 
-    private List<MatOfPoint> foundContours_ = new ArrayList<>();
-    private List<MatOfPoint> filteredContours_ = new ArrayList<>();
-    private List<MatOfPoint> deSpeckledContours_ = new ArrayList<>();
-    private List<RotatedRect> groupedContours_ = new ArrayList<>();
+    private USBCamera camera;
 
-    public CVPipeline2d(CVPipelineSettings settings) {
+    private Mat rawCameraMat = new Mat();
+    private Mat hsvOutputMat = new Mat();
+
+    public CVPipeline2d(CVPipeline2dSettings settings) {
         super(settings);
     }
 
     @Override
-    void initPipeline() {
-
+    void initPipeline(USBCamera cam) {
+        camera = cam;
     }
 
     @Override
     CVPipeline2d.CVPipeline2dResult runPipeline(Mat inputMat) {
-        var shouldFlip = settings.flipMode.equals(ImageFlipMode.BOTH);
-        var result = new CVPipeline2dResult();
+        long totalProcessTimeNanos = 0;
+        StringBuilder procTimeStringBuilder = new StringBuilder();
 
-        // flip the image
-        if (shouldFlip) {
-            Core.flip(inputMat, inputMat, -1);
-        }
+        CameraStaticProperties camProps = camera.properties.staticProperties;
 
-        foundContours_.clear();
-        filteredContours_.clear();
-        deSpeckledContours_.clear();
-        groupedContours_.clear();
+        inputMat.copyTo(rawCameraMat);
+
+        // prepare pipes
+        RotateFlipPipe rotateFlipPipe = new RotateFlipPipe(ImageRotation.DEG_0, settings.flipMode);
+        BlurPipe blurPipe = new BlurPipe(5);
+        ErodeDilatePipe erodeDilatePipe = new ErodeDilatePipe(settings.erode, settings.dilate, 7);
 
-        // HSV threshold the image
         Scalar hsvLower = new Scalar(settings.hue.get(0).intValue(), settings.saturation.get(0).intValue(), settings.value.get(0).intValue());
         Scalar hsvUpper = new Scalar(settings.hue.get(1).intValue(), settings.saturation.get(1).intValue(), settings.value.get(1).intValue());
-        hsvThreshold(inputImage, hsvThreshMat, settings.erode, settings.dilate);
 
-        // Make sure we're BFR
-        if (settings.isBinary) {
-            Imgproc.cvtColor(hsvThreshMat, outputImage, Imgproc.COLOR_GRAY2BGR, 3);
-        } else {
-            inputImage.copyTo(outputImage);
-        }
+        HsvPipe hsvPipe = new HsvPipe(hsvLower, hsvUpper);
 
-        // search for contours
-        foundContours_ = findContours(hsvThreshMat);
-        if (foundContours_.size() < 1) {
-            return result;
-        }
+        FindContoursPipe findContoursPipe = new FindContoursPipe();
+        FilterContoursPipe filterContoursPipe = new FilterContoursPipe(settings.area, settings.ratio, settings.extent, camProps);
+        SpeckleRejectPipe speckleRejectPipe = new SpeckleRejectPipe(settings.speckle.doubleValue());
+        GroupContoursPipe groupContoursPipe = new GroupContoursPipe(settings.targetGroup, settings.targetIntersection);
+        SortContoursPipe sortContoursPipe = new SortContoursPipe(settings.sortMode, camProps);
+        Collect2dTargetsPipe collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode, settings.point,
+                settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps);
 
-        // filter contours by area, ratio and extent
-        filteredContours_ = filterContours(foundContours_, settings.area, settings.ratio, settings.extent);
-        if (filteredContours_.size() < 1) {
-            return result;
-        }
+        OutputMatPipe outputMatPipe = new OutputMatPipe(settings.isBinary);
 
-        // reject "speckle" contours
-        deSpeckledContours_ = rejectSpeckles(filteredContours_, settings.speckle.doubleValue());
-        if (deSpeckledContours_.size() < 1) {
-            return result;
-        }
+        Draw2dContoursPipe.Draw2dContoursSettings draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings();
+        draw2dContoursSettings.showCentroid = false;
+        draw2dContoursSettings.showCrosshair = true;
+        draw2dContoursSettings.boxOutlineSize = 2;
+        draw2dContoursSettings.showRotatedBox = true;
+        draw2dContoursSettings.showMaximumBox = true;
 
-        // group targets
-        groupedContours_ = groupTargets(deSpeckledContours_, settings.targetIntersection, settings.targetGroup);
-        if (groupedContours_.size() < 1) {
-            return result;
-        }
+        Draw2dContoursPipe draw2dContoursPipe = new Draw2dContoursPipe(draw2dContoursSettings, camProps);
 
-        // sort targets down to our final target
-        var finalRect = sortTargetsToOne(groupedContours_, settings.sortMode);
-        result.RawPoint = finalRect;
-        result.IsValid = true;
-        switch (settings.calibrationMode) {
-            case None:
-                ///use the center of the USBCamera to find the pitch and yaw difference
-                result.CalibratedX = cameraValues.CenterX;
-                result.CalibratedY = cameraValues.CenterY;
-                break;
-            case Single:
-                // use the static point as a calibration method instead of the center
-                result.CalibratedX = settings.point.get(0).doubleValue();
-                result.CalibratedY = settings.point.get(1).doubleValue();
-                break;
-            case Dual:
-                // use the calculated line to find the difference in length between the point and the line
-                result.CalibratedX = (finalRect.center.y - settings.b) / settings.m;
-                result.CalibratedY = (finalRect.center.x * settings.m) + settings.b;
-                break;
-        }
+        // run pipes
+        Pair<Mat, Long> rotateFlipResult = rotateFlipPipe.run(inputMat);
+        totalProcessTimeNanos += rotateFlipResult.getRight();
+        procTimeStringBuilder.append(String.format("RotateFlip: %.2fms, ", rotateFlipResult.getRight() / 1000.0));
 
-        result.Pitch = cameraValues.CalculatePitch(finalRect.center.y, result.CalibratedY);
-        result.Yaw = cameraValues.CalculateYaw(finalRect.center.x, result.CalibratedX);
-        result.Area = finalRect.size.area();
-        drawContour(outputImage, finalRect);
+        Pair<Mat, Long> blurResult = blurPipe.run(rotateFlipResult.getLeft());
+        totalProcessTimeNanos += blurResult.getRight();
+        procTimeStringBuilder.append(String.format("Blur: %.2fms, ", blurResult.getRight() / 1000.0));
 
-        return result;
-    }
+        Pair<Mat, Long> erodeDilateResult = erodeDilatePipe.run(blurResult.getLeft());
+        totalProcessTimeNanos += erodeDilateResult.getRight();
+        procTimeStringBuilder.append(String.format("ErodeDilate: %.2fms, ", erodeDilateResult.getRight() / 1000.0));
 
-    /**
-     * HSV Threshold a given image. Copies the HSV Thresholded image to the [dst] matrix with the given
-     * hsv settings and blur settings. Can also erode and dilate the image
-     * @param srcImage the source image, which is not mutated
-     * @param dst the destination image, which is mutated to save the result
-     * @param hsvLower the lower bound for the HSV settings
-     * @param hsvUpper the upper bound for the HSV settings
-     * @param kernel the kernal used to erode/dilate the image
-     * @param blur the size of the blur image
-     * @param shouldErode if we should erode
-     * @param shouldDilate if we should dilate
-     */
-    public static void hsvThreshold(Mat srcImage, Mat dst, @NotNull Scalar hsvLower,
-                                    @NotNull Scalar hsvUpper, Mat kernel, Size blur,
-                                    boolean shouldErode, boolean shouldDilate) {
-        Imgproc.cvtColor(srcImage, dst, Imgproc.COLOR_RGB2HSV, 3);
-        Imgproc.blur(dst, dst, blur);
-        Core.inRange(dst, hsvLower, hsvUpper, dst);
-        if (shouldErode) {
-            Imgproc.erode(dst, dst, kernel);
-        }
-        if (shouldDilate) {
-            Imgproc.dilate(dst, dst, kernel);
-        }
-        dst.release();
-    }
+        Pair<Mat, Long> hsvResult = hsvPipe.run(erodeDilateResult.getLeft());
+        totalProcessTimeNanos += hsvResult.getRight();
+        Imgproc.cvtColor(hsvResult.getLeft(), hsvOutputMat, Imgproc.COLOR_GRAY2BGR, 3);
+        procTimeStringBuilder.append(String.format("HSV: %.2fms, ", hsvResult.getRight() / 1000.0));
 
-    /**
-     * Find contours from an image
-     * @param src the image we're looking at
-     * @param binaryMat a temporary image
-     * @param hierarchy the hierarchy of the image (just a new Mat();)
-     * @param emptyList a list to fill with stuff. Will be cleared
-     * @return the empty list, now full of contours
-     */
-    public static List<MatOfPoint> findContours(Mat src, Mat binaryMat, Mat hierarchy, List<MatOfPoint> emptyList) {
-        src.copyTo(binaryMat);
-        emptyList.clear();
-        Imgproc.findContours(binaryMat, emptyList, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1);
-        binaryMat.release();
-        return emptyList;
-    }
+        Pair<List<MatOfPoint>, Long> findContoursResult = findContoursPipe.run(hsvResult.getLeft());
+        totalProcessTimeNanos += findContoursResult.getRight();
+        procTimeStringBuilder.append(String.format("FindContours: %.2fms, ", findContoursResult.getRight() / 1000.0));
 
-    public static List<MatOfPoint> filterContours(List<MatOfPoint> inputContours, List<Number> area, List<Number> ratio, List<Number> extent, CameraValues cameraValues) {
-        for (MatOfPoint Contour : inputContours) {
-            try {
-                double contourArea = Imgproc.contourArea(Contour);
-                double AreaRatio = (contourArea / cameraValues.ImageArea) * 100;
-                double minArea = (MathHandler.sigmoid(area.get(0)));
-                double maxArea = (MathHandler.sigmoid(area.get(1)));
-                if (AreaRatio < minArea || AreaRatio > maxArea) {
-                    continue;
-                }
-                var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray()));
+        Pair<List<MatOfPoint>, Long> filterContoursResult = filterContoursPipe.run(findContoursResult.getLeft());
+        totalProcessTimeNanos += filterContoursResult.getRight();
+        procTimeStringBuilder.append(String.format("FilterContours: %.2fms, ", filterContoursResult.getRight() / 1000.0));
 
-                var targetFullness = contourArea;
-                double minExtent = (double) (extent.get(0).doubleValue() * rect.size.area()) / 100;
-                double maxExtent = (double) (extent.get(1).doubleValue() * rect.size.area()) / 100;
-                if (targetFullness <= minExtent || contourArea >= maxExtent) {
-                    continue;
-                }
-                Rect bb = Imgproc.boundingRect(Contour);
-                double aspectRatio = (bb.width / bb.height);
-                if (aspectRatio < ratio.get(0).doubleValue() || aspectRatio > ratio.get(1).doubleValue()) {
-                    continue;
-                }
-                filteredContours.add(Contour);
-            } catch (Exception e) {
-                System.err.println("Error while filtering contours");
-                e.printStackTrace();
-            }
-        }
-        return filteredContours;
-    }
+        Pair<List<MatOfPoint>, Long> speckleRejectResult = speckleRejectPipe.run(filterContoursResult.getLeft());
+        totalProcessTimeNanos += speckleRejectResult.getRight();
+        procTimeStringBuilder.append(String.format("SpeckleReject: %.2fms, ", speckleRejectResult.getRight() / 1000.0));
 
-    @Override
-    Mat getOutputMat() {
-        return null;
+        Pair<List<RotatedRect>, Long> groupContoursResult = groupContoursPipe.run(speckleRejectResult.getLeft());
+        totalProcessTimeNanos += groupContoursResult.getRight();
+        procTimeStringBuilder.append(String.format("GroupContours: %.2fms, ", groupContoursResult.getRight() / 1000.0));
+
+        Pair<List<RotatedRect>, Long> sortContoursResult = sortContoursPipe.run(groupContoursResult.getLeft());
+        totalProcessTimeNanos += sortContoursResult.getRight();
+        procTimeStringBuilder.append(String.format("SortContours: %.2fms, ", sortContoursResult.getRight() / 1000.0));
+
+        Pair<List<Target>, Long> collect2dTargetsResult = collect2dTargetsPipe.run(sortContoursResult.getLeft());
+        totalProcessTimeNanos += collect2dTargetsResult.getRight();
+        procTimeStringBuilder.append(String.format("SortContours: %.2fms, ", sortContoursResult.getRight() / 1000.0));
+
+        // takes pair of (Mat of original camera image, Mat of HSV thresholded image)
+        Pair<Mat, Long> outputMatResult = outputMatPipe.run(Pair.of(rawCameraMat, hsvOutputMat));
+        totalProcessTimeNanos += outputMatResult.getRight();
+        procTimeStringBuilder.append(String.format("OutputMat: %.2fms, ", outputMatResult.getRight() / 1000.0));
+
+        // takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
+        Pair<Mat, Long> draw2dContoursResult = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft()));
+        totalProcessTimeNanos += draw2dContoursResult.getRight();
+        procTimeStringBuilder.append(String.format("Draw2dContours: %.2fms, ", draw2dContoursResult.getRight() / 1000.0));
+
+        System.out.println(procTimeStringBuilder.toString());
+        System.out.printf("Pipeline ran in %.3fms\n", totalProcessTimeNanos / 1000.0);
+
+        return new CVPipeline2dResult(collect2dTargetsResult.getLeft(), draw2dContoursResult.getLeft());
     }
 
     public static class CVPipeline2dSettings extends CVPipelineSettings {
@@ -193,27 +128,20 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2d.CVPipeline2dResult> {
         double dualTargetCalibrationB = 0;
     }
 
-    public static class CVPipeline2dResult {
-        public boolean hasTarget = false;
-        public ArrayList<Target> targets = new ArrayList<>(); // targets sorted by likelihood
-
-        public CVPipeline2dResult(ArrayList<Target> targets, boolean hasTarget) {
+    public static class CVPipeline2dResult extends CVPipelineResult<Target> {
+        public CVPipeline2dResult(List<Target> targets, Mat outputMat) {
             this.targets = targets;
-            this.hasTarget = hasTarget;
-        }
-
-        public CVPipeline2dResult() {
+            this.hasTarget = !targets.isEmpty();
+            this.outputMat = outputMat;
         }
     }
 
     public static class Target {
-        public boolean isValid = false;
         public double calibratedX = 0.0;
         public double calibratedY = 0.0;
         public double pitch = 0.0;
         public double yaw = 0.0;
         public double area = 0.0;
-        RotatedRect rawPoint;
+        public RotatedRect rawPoint;
     }
-
 }

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline3d.java

@@ -1,33 +1,33 @@
-package com.chameleonvision.classabstraction.pipeline;
-
-import org.opencv.core.Mat;
-
-public class CVPipeline3d extends CVPipeline<CVPipeline3d.CVPipeline3dResult> {
-
-    public CVPipeline3d(CVPipelineSettings settings) {
-        super(settings);
-    }
-
-    @Override
-    void initPipeline() {
-
-    }
-
-    @Override
-    CVPipeline3d.CVPipeline3dResult runPipeline(Mat inputMat) {
-        return null;
-    }
-
-    @Override
-    Mat getOutputMat() {
-        return null;
-    }
-
-    public static class CVPipeline3dSettings extends CVPipelineSettings {
-    }
-
-    public static class CVPipeline3dResult {
-
-    }
-
-}
+//package com.chameleonvision.classabstraction.pipeline;
+//
+//import org.opencv.core.Mat;
+//
+//public class CVPipeline3d extends CVPipeline<CVPipeline3d.CVPipeline3dResult> {
+//
+//    public CVPipeline3d(CVPipelineSettings settings) {
+//        super(settings);
+//    }
+//
+//    @Override
+//    void initPipeline() {
+//
+//    }
+//
+//    @Override
+//    CVPipeline3d.CVPipeline3dResult runPipeline(Mat inputMat) {
+//        return null;
+//    }
+//
+//    @Override
+//    Mat getOutputMat() {
+//        return null;
+//    }
+//
+//    public static class CVPipeline3dSettings extends CVPipelineSettings {
+//    }
+//
+//    public static class CVPipeline3dResult {
+//
+//    }
+//
+//}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipelineResult.java

@@ -0,0 +1,11 @@
+package com.chameleonvision.classabstraction.pipeline;
+
+import org.opencv.core.Mat;
+
+import java.util.List;
+
+public abstract class CVPipelineResult<T> {
+    List<T> targets;
+    boolean hasTarget;
+    Mat outputMat;
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/DriverVisionPipeline.java

@@ -1,25 +1,29 @@
 package com.chameleonvision.classabstraction.pipeline;
 
+import com.chameleonvision.classabstraction.camera.USBCamera;
+import com.chameleonvision.vision.process.PipelineResult;
 import org.opencv.core.Mat;
 
-public class DriverVisionPipeline extends CVPipeline<Void> {
+public class DriverVisionPipeline extends CVPipeline<DriverVisionPipeline.DriverPipelineResult, CVPipelineSettings> {
     public DriverVisionPipeline(CVPipelineSettings settings) {
         super(settings);
     }
 
     @Override
-    void initPipeline() {
-        // TODO set exposure/brightness of camera
+    void initPipeline(USBCamera camera) {
+        // TODO: set camera to driver mode
     }
 
     @Override
-    Void runPipeline(Mat inputMat) {
-        this.outputMat = inputMat;
-        return null;
+    DriverPipelineResult runPipeline(Mat inputMat) {
+        return new DriverPipelineResult(inputMat);
     }
 
-    @Override
-    Mat getOutputMat() {
-        return this.outputMat;
+    public static class DriverPipelineResult extends CVPipelineResult<Void> {
+        public DriverPipelineResult(Mat outputMat) {
+            this.hasTarget = false;
+            this.targets = null;
+            outputMat.copyTo(this.outputMat);
+        }
     }
 }

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/BlurPipe.java

@@ -0,0 +1,31 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+public class BlurPipe implements Pipe<Mat, Mat> {
+
+    private final int blurSize;
+
+    private Mat outputMat = new Mat();
+
+    public BlurPipe(int blurSize) {
+        this.blurSize = blurSize;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Mat input) {
+        long processStartNanos = System.nanoTime();
+
+        if (blurSize > 0) {
+            Imgproc.blur(outputMat, outputMat, new Size(blurSize, blurSize));
+        }
+
+        long processTime = processStartNanos - System.nanoTime();
+        Pair<Mat, Long> output = Pair.of(outputMat, processTime);
+        outputMat.release();
+        return output;
+    }
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/Collect2dTargetsPipe.java

@@ -0,0 +1,71 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import com.chameleonvision.classabstraction.camera.CameraStaticProperties;
+import com.chameleonvision.classabstraction.pipeline.CVPipeline2d;
+import com.chameleonvision.vision.CalibrationMode;
+import org.apache.commons.lang3.tuple.Pair;
+import org.apache.commons.math3.util.FastMath;
+import org.opencv.core.RotatedRect;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class Collect2dTargetsPipe implements Pipe<List<RotatedRect>, List<CVPipeline2d.Target>> {
+
+    private final CalibrationMode calibrationMode;
+    private final CameraStaticProperties camProps;
+    private final List<Number> calibrationPoint;
+    private final double calibrationM, calibrationB;
+
+    private List<CVPipeline2d.Target> targets = new ArrayList<>();
+
+    public Collect2dTargetsPipe(CalibrationMode calibrationMode, List<Number> calibrationPoint, double calibrationM, double calibrationB, CameraStaticProperties camProps) {
+        this.calibrationMode = calibrationMode;
+        this.camProps = camProps;
+        this.calibrationPoint = calibrationPoint;
+        this.calibrationM = calibrationM;
+        this.calibrationB = calibrationB;
+    }
+
+    @Override
+    public Pair<List<CVPipeline2d.Target>, Long> run(List<RotatedRect> input) {
+        long processStartNanos = System.nanoTime();
+
+        input.forEach(r -> {
+            CVPipeline2d.Target t = new CVPipeline2d.Target();
+            t.rawPoint = r;
+            switch (calibrationMode) {
+                case None:
+                    t.calibratedX = camProps.centerX;
+                    t.calibratedY = camProps.centerY;
+                    break;
+                case Single:
+                    t.calibratedX = calibrationPoint.get(0).doubleValue();
+                    t.calibratedY = calibrationPoint.get(1).doubleValue();
+                    break;
+                case Dual:
+                    t.calibratedX = (r.center.y - calibrationB) / calibrationM;
+                    t.calibratedY = (r.center.x * calibrationM) + calibrationB;
+                    break;
+            }
+
+            t.pitch = calculatePitch(r.center.y, t.calibratedY);
+            t.yaw = calculateYaw(r.center.x, t.calibratedX);
+            t.area = r.size.area();
+
+            targets.add(t);
+        });
+
+        long processTime = processStartNanos - System.nanoTime();
+        return Pair.of(targets, processTime);
+    }
+
+    private double calculatePitch(double pixelY, double centerY) {
+        double pitch = FastMath.toDegrees(FastMath.atan((pixelY - centerY) / camProps.VerticalFocalLength));
+        return (pitch * -1);
+    }
+
+    private double calculateYaw(double pixelX, double centerX) {
+        return FastMath.toDegrees(FastMath.atan((pixelX - centerX) / camProps.HorizontalFocalLength));
+    }
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/Draw2dContoursPipe.java

@@ -0,0 +1,85 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import com.chameleonvision.classabstraction.camera.CameraStaticProperties;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.*;
+import org.opencv.core.Point;
+import org.opencv.imgproc.Imgproc;
+
+import java.awt.*;
+import java.util.ArrayList;
+import java.util.List;
+
+public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Mat> {
+
+    private final Draw2dContoursSettings settings;
+    private final CameraStaticProperties camProps;
+
+    private Mat outputMat = new Mat();
+
+    public Draw2dContoursPipe(Draw2dContoursSettings settings, CameraStaticProperties camProps) {
+        this.settings = settings;
+        this.camProps = camProps;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Pair<Mat, List<RotatedRect>> input) {
+        long processStartNanos = System.nanoTime();
+
+        input.getLeft().copyTo(outputMat);
+
+        for (RotatedRect r : input.getRight()) {
+            if (r == null) continue;
+
+            List<MatOfPoint> drawnContour = new ArrayList<>();
+            Point[] vertices = new Point[4];
+            r.points(vertices);
+            MatOfPoint contour = new MatOfPoint(vertices);
+            drawnContour.add(contour);
+
+            if (settings.showCentroid) {
+                Imgproc.circle(outputMat, r.center, 3, colorToScalar(settings.centroidColor));
+            }
+
+            if (settings.showCrosshair) {
+                Point xMax = new Point(camProps.centerX + 10, camProps.centerY);
+                Point xMin = new Point(camProps.centerX - 10, camProps.centerY);
+                Point yMax = new Point(camProps.centerX, camProps.centerY + 10);
+                Point yMin = new Point(camProps.centerX, camProps.centerY - 10);
+                Imgproc.line(outputMat, xMax, xMin, colorToScalar(settings.crosshairColor), 2);
+                Imgproc.line(outputMat, yMax, yMin, colorToScalar(settings.crosshairColor), 2);
+            }
+
+            if (settings.showRotatedBox) {
+                Imgproc.drawContours(outputMat, drawnContour, 0, colorToScalar(settings.rotatedBoxColor), settings.boxOutlineSize);
+            }
+
+            if (settings.showMaximumBox) {
+                Rect box = Imgproc.boundingRect(contour);
+                Imgproc.rectangle(outputMat, new Point(box.x, box.y), new Point((box.x + box.width), (box.y + box.height)), colorToScalar(settings.maximumBoxColor), settings.boxOutlineSize);
+            }
+        }
+
+        long processTime = processStartNanos - System.nanoTime();
+        Pair<Mat, Long> output = Pair.of(outputMat, processTime);
+        outputMat.release();
+        return output;
+    }
+
+    private Scalar colorToScalar(Color color) {
+        return new Scalar(color.getRed(), color.getGreen(), color.getBlue());
+    }
+
+    public static class Draw2dContoursSettings {
+        public boolean showCentroid = false;
+        public boolean showCrosshair = false;
+        public int boxOutlineSize = 0;
+        public boolean showRotatedBox = false;
+        public boolean showMaximumBox = false;
+        public Color centroidColor = Color.GREEN;
+        public Color crosshairColor = Color.GREEN;
+        public Color rotatedBoxColor = Color.BLUE;
+        public Color maximumBoxColor = Color.RED;
+
+    }
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/ErodeDilatePipe.java

@@ -0,0 +1,38 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+public class ErodeDilatePipe implements Pipe<Mat, Mat> {
+
+    private final boolean erode, dilate;
+    private final Mat kernel;
+
+    private Mat outputMat = new Mat();
+
+    public ErodeDilatePipe(boolean erode, boolean dilate, int kernelSize) {
+        this.erode = erode;
+        this.dilate = dilate;
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(kernelSize, kernelSize));
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Mat input) {
+        long processStartNanos = System.nanoTime();
+
+        if (erode) {
+            Imgproc.erode(outputMat, outputMat, kernel);
+        }
+
+        if (dilate) {
+            Imgproc.erode(outputMat, outputMat, kernel);
+        }
+
+        long processTime = processStartNanos - System.nanoTime();
+        Pair<Mat, Long> output = Pair.of(outputMat, processTime);
+        outputMat.release();
+        return output;
+    }
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/FilterContoursPipe.java

@@ -0,0 +1,62 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import com.chameleonvision.classabstraction.camera.CameraStaticProperties;
+import com.chameleonvision.util.MathHandler;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Rect;
+import org.opencv.imgproc.Imgproc;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class FilterContoursPipe implements Pipe<List<MatOfPoint>, List<MatOfPoint>> {
+
+    private final List<Number> area, ratio, extent;
+    private final CameraStaticProperties camProps;
+
+    private List<MatOfPoint> filteredContours = new ArrayList<>();
+
+    public FilterContoursPipe(List<Number> area, List<Number> ratio, List<Number> extent, CameraStaticProperties camProps) {
+        this.area = area;
+        this.ratio = ratio;
+        this.extent = extent;
+        this.camProps = camProps;
+    }
+
+    @Override
+    public Pair<List<MatOfPoint>, Long> run(List<MatOfPoint> input) {
+        long processStartNanos = System.nanoTime();
+
+        for (MatOfPoint Contour : input) {
+            try {
+                double contourArea = Imgproc.contourArea(Contour);
+                double AreaRatio = (contourArea / camProps.ImageArea) * 100;
+                double minArea = (MathHandler.sigmoid(area.get(0)));
+                double maxArea = (MathHandler.sigmoid(area.get(1)));
+                if (AreaRatio < minArea || AreaRatio > maxArea) {
+                    continue;
+                }
+                var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray()));
+                double minExtent = (extent.get(0).doubleValue() * rect.size.area()) / 100;
+                double maxExtent = (extent.get(1).doubleValue() * rect.size.area()) / 100;
+                if (contourArea <= minExtent || contourArea >= maxExtent) {
+                    continue;
+                }
+                Rect bb = Imgproc.boundingRect(Contour);
+                double aspectRatio = ((double)bb.width / bb.height);
+                if (aspectRatio < ratio.get(0).doubleValue() || aspectRatio > ratio.get(1).doubleValue()) {
+                    continue;
+                }
+                filteredContours.add(Contour);
+            } catch (Exception e) {
+                System.err.println("Error while filtering contours");
+                e.printStackTrace();
+            }
+        }
+
+        long processTime = processStartNanos - System.nanoTime();
+        return Pair.of(filteredContours, processTime);
+    }
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/FindContoursPipe.java

@@ -0,0 +1,26 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.imgproc.Imgproc;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class FindContoursPipe implements Pipe<Mat, List<MatOfPoint>> {
+
+    private List<MatOfPoint> foundContours = new ArrayList<>();
+
+    public FindContoursPipe() {}
+
+    @Override
+    public Pair<List<MatOfPoint>, Long> run(Mat input) {
+        long processStartNanos = System.nanoTime();
+
+        Imgproc.findContours(input, foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1);
+
+        long processTime = processStartNanos - System.nanoTime();
+        return Pair.of(foundContours, processTime);
+    }
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/GroupContoursPipe.java

@@ -0,0 +1,154 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import com.chameleonvision.util.MathHandler;
+import com.chameleonvision.vision.TargetGroup;
+import com.chameleonvision.vision.TargetIntersection;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.*;
+import org.opencv.imgproc.Imgproc;
+import org.opencv.imgproc.Moments;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
+
+public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRect>> {
+
+    private static final Comparator<MatOfPoint> sortByMomentsX =
+            Comparator.comparingDouble(GroupContoursPipe::calcMomentsX);
+
+    private final TargetGroup group;
+    private final TargetIntersection intersection;
+
+    private List<RotatedRect> groupedContours = new ArrayList<>();
+    private MatOfPoint2f intersectMatA = new MatOfPoint2f();
+    private MatOfPoint2f intersectMatB = new MatOfPoint2f();
+
+    public GroupContoursPipe(TargetGroup group, TargetIntersection intersection) {
+        this.group = group;
+        this.intersection = intersection;
+    }
+
+    @Override
+    public Pair<List<RotatedRect>, Long> run(List<MatOfPoint> input) {
+        long processStartNanos = System.nanoTime();
+
+        List<MatOfPoint> sorted = new ArrayList<>(input);
+        sorted.sort(sortByMomentsX);
+
+        Collections.reverse(sorted);
+
+        switch (group) {
+            case Single: {
+                input.forEach(c -> {
+                    MatOfPoint2f contour = new MatOfPoint2f();
+                    contour.fromArray(c.toArray());
+                    if (contour.cols() != 0 && contour.rows() != 0) {
+                        RotatedRect rect = Imgproc.minAreaRect(contour);
+                        groupedContours.add(rect);
+                    }
+                });
+                break;
+            }
+            case Dual: {
+                for (var i = 0; i < input.size(); i++) {
+                    List<Point> finalContourList = new ArrayList<>(input.get(i).toList());
+
+                    try {
+                        MatOfPoint firstContour = input.get(i);
+                        MatOfPoint secondContour = input.get(i + 1);
+
+                        if (isIntersecting(firstContour, secondContour)) {
+                            finalContourList.addAll(secondContour.toList());
+                        } else {
+                            finalContourList.clear();
+                            continue;
+                        }
+
+                        firstContour.release();
+                        secondContour.release();
+
+                        MatOfPoint2f contour = new MatOfPoint2f();
+                        contour.fromList(finalContourList);
+
+                        if (contour.cols() != 0 && contour.rows() != 0) {
+                            RotatedRect rect = Imgproc.minAreaRect(contour);
+                            groupedContours.add(rect);
+                        }
+                    } catch (IndexOutOfBoundsException e) {
+                        System.err.println("GroupContours: WTF");
+                        finalContourList.clear();
+                    }
+                }
+                break;
+            }
+        }
+
+        long processTime = processStartNanos - System.nanoTime();
+        return Pair.of(groupedContours, processTime);
+    }
+
+    private static double calcMomentsX(MatOfPoint c) {
+        Moments m = Imgproc.moments(c);
+        return (m.get_m10() / m.get_m00());
+    }
+
+    private boolean isIntersecting(MatOfPoint contourOne, MatOfPoint contourTwo) {
+        if (intersection.equals(TargetIntersection.None)) {
+            return true;
+        }
+
+        try {
+            intersectMatA.fromArray(contourOne.toArray());
+            intersectMatB.fromArray(contourTwo.toArray());
+            RotatedRect a = Imgproc.fitEllipse(intersectMatA);
+            RotatedRect b = Imgproc.fitEllipse(intersectMatB);
+            double mA = MathHandler.toSlope(a.angle);
+            double mB = MathHandler.toSlope(b.angle);
+            double x0A = a.center.x;
+            double y0A = a.center.y;
+            double x0B = b.center.x;
+            double y0B = b.center.y;
+            double intersectionX = ((mA * x0A) - y0A - (mB * x0B) + y0B) / (mA - mB);
+            double intersectionY = (mA * (intersectionX - x0A)) + y0A;
+            double massX = (x0A + x0B) / 2;
+            double massY = (y0A + y0B) / 2;
+            switch (intersection) {
+                case Up: {
+                    if (intersectionY < massY) {
+                        if (mA > 0 && mB < 0) {
+                            return true;
+                        }
+                    }
+                    break;
+                }
+                case Down: {
+                    if (intersectionY > massY) {
+                        if (mA < 0 && mB > 0) {
+                            return true;
+                        }
+                    }
+
+                    break;
+                }
+                case Left: {
+                    if (intersectionX < massX) {
+
+                        return true;
+                    }
+                    break;
+                }
+                case Right: {
+                    if (intersectionX > massX) {
+                        return true;
+                    }
+                    break;
+                }
+            }
+            return false;
+        } catch (Exception e) {
+            return false;
+        }
+    }
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/HsvPipe.java

@@ -0,0 +1,35 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+public class HsvPipe implements Pipe<Mat, Mat> {
+
+    private final Scalar hsvLower, hsvUpper;
+
+    private Mat outputMat = new Mat();
+
+    public HsvPipe(Scalar hsvLower, Scalar hsvUpper) {
+        this.hsvLower = hsvLower;
+        this.hsvUpper = hsvUpper;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Mat input) {
+        long processStartNanos = System.nanoTime();
+
+        Imgproc.cvtColor(input, outputMat, Imgproc.COLOR_RGB2HSV, 3);
+
+        Core.inRange(outputMat, hsvLower, hsvUpper, outputMat);
+
+        long processTime = processStartNanos - System.nanoTime();
+        Pair<Mat, Long> output = Pair.of(outputMat, processTime);
+        outputMat.release();
+        return output;
+    }
+}
+

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/OutputMatPipe.java

@@ -0,0 +1,31 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+
+public class OutputMatPipe implements Pipe<Pair<Mat, Mat>, Mat> {
+
+    private boolean showThresholded;
+
+    private Mat outputMat = new Mat();
+
+    public OutputMatPipe(boolean showThresholded) {
+        this.showThresholded = showThresholded;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Pair<Mat, Mat> input) {
+        long processStartNanos = System.nanoTime();
+
+        if (showThresholded) {
+            input.getRight().copyTo(outputMat);
+        } else {
+            input.getLeft().copyTo(outputMat);
+        }
+
+        long processTime = processStartNanos - System.nanoTime();
+        Pair<Mat, Long> output = Pair.of(outputMat, processTime);
+        outputMat.release();
+        return output;
+    }
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/Pipe.java

@@ -0,0 +1,13 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+
+public interface Pipe<I, O> {
+    /**
+     *
+     * @param input Input object for pipe
+     * @return Returns a Pair containing the process time in Nanoseconds,
+     *         and the output object
+     */
+    Pair<O, Long> run(I input);
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/RotateFlipPipe.java

@@ -0,0 +1,33 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import com.chameleonvision.vision.ImageFlipMode;
+import com.chameleonvision.vision.ImageRotation;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+
+public class RotateFlipPipe implements Pipe<Mat, Mat> {
+
+    private final ImageRotation rotation;
+    private final ImageFlipMode flip;
+
+    private Mat outputMat = new Mat();
+
+    public RotateFlipPipe(ImageRotation rotation, ImageFlipMode flip) {
+        this.rotation = rotation;
+        this.flip = flip;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Mat input) {
+        long processStartNanos = System.nanoTime();
+
+        Core.flip(input, outputMat, flip.value);
+        Core.rotate(outputMat, outputMat, rotation.value);
+
+        long processTime = processStartNanos - System.nanoTime();
+        Pair<Mat, Long> output = Pair.of(outputMat, processTime);
+        outputMat.release();
+        return output;
+    }
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/SortContoursPipe.java

@@ -0,0 +1,74 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import com.chameleonvision.classabstraction.camera.CameraStaticProperties;
+import com.chameleonvision.vision.SortMode;
+import org.apache.commons.lang3.tuple.Pair;
+import org.apache.commons.math3.util.FastMath;
+import org.opencv.core.RotatedRect;
+
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+
+public class SortContoursPipe implements Pipe<List<RotatedRect>, List<RotatedRect>> {
+
+    private final Comparator<RotatedRect> SortByCentermostComparator = Comparator.comparingDouble(this::calcCenterDistance);
+
+    private static final Comparator<RotatedRect> SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.size.area(), rect1.size.area());
+    private static final Comparator<RotatedRect> SortBySmallestComparator = SortByLargestComparator.reversed();
+
+    private static final Comparator<RotatedRect> SortByHighestComparator = (rect1, rect2) -> Double.compare(rect2.center.y, rect1.center.y);
+    private static final Comparator<RotatedRect> SortByLowestComparator = SortByHighestComparator.reversed();
+
+    private static final Comparator<RotatedRect> SortByLeftmostComparator = Comparator.comparingDouble(rect -> rect.center.x);
+    private static final Comparator<RotatedRect> SortByRightmostComparator = SortByLeftmostComparator.reversed();
+
+
+    private final SortMode sort;
+    private final CameraStaticProperties camProps;
+
+    private List<RotatedRect> sortedContours = new ArrayList<>();
+
+    public SortContoursPipe(SortMode sort, CameraStaticProperties camProps) {
+        this.sort = sort;
+        this.camProps = camProps;
+    }
+
+    @Override
+    public Pair<List<RotatedRect>, Long> run(List<RotatedRect> input) {
+        long processStartNanos = System.nanoTime();
+
+        switch (sort) {
+            case Largest:
+                input.sort(SortByLargestComparator);
+                break;
+            case Smallest:
+                input.sort(SortBySmallestComparator);
+                break;
+            case Highest:
+                input.sort(SortByHighestComparator);
+                break;
+            case Lowest:
+                input.sort(SortByLowestComparator);
+                break;
+            case Leftmost:
+                input.sort(SortByLeftmostComparator);
+                break;
+            case Rightmost:
+                input.sort(SortByRightmostComparator);
+                break;
+            case Centermost:
+                input.sort(SortByCentermostComparator);
+                break;
+            default:
+                break;
+        }
+
+        long processTime = processStartNanos - System.nanoTime();
+        return Pair.of(sortedContours, processTime);
+    }
+
+    private double calcCenterDistance(RotatedRect rect) {
+        return FastMath.sqrt(FastMath.pow(camProps.centerX - rect.center.x, 2) + FastMath.pow(camProps.centerY - rect.center.y, 2));
+    }
+}

Main/src/main/java/com/chameleonvision/classabstraction/pipeline/pipes/SpeckleRejectPipe.java

@@ -0,0 +1,44 @@
+package com.chameleonvision.classabstraction.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.imgproc.Imgproc;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class SpeckleRejectPipe implements Pipe<List<MatOfPoint>, List<MatOfPoint>> {
+
+    private final double minPercentOfAvg;
+
+    private List<MatOfPoint> despeckledContours = new ArrayList<>();
+
+    public SpeckleRejectPipe(double minPercentOfAvg) {
+        this.minPercentOfAvg = minPercentOfAvg;
+    }
+
+    @Override
+    public Pair<List<MatOfPoint>, Long> run(List<MatOfPoint> input) {
+        long processStartNanos = System.nanoTime();
+
+        double averageArea = 0.0;
+
+        for (MatOfPoint c : input) {
+            averageArea += Imgproc.contourArea(c);
+        }
+
+        averageArea /= input.size();
+
+        double minAllowedArea = minPercentOfAvg / 100.0 * averageArea;
+
+        for (MatOfPoint c : input) {
+            if (Imgproc.contourArea(c) >= minAllowedArea) {
+                despeckledContours.add(c);
+            }
+        }
+
+        long processTime = processStartNanos - System.nanoTime();
+        return Pair.of(despeckledContours, processTime);
+    }
+}