Move Java backend to properly named folder

chameleon-server/src/main/java/com/chameleonvision/vision/VisionManager.java

@@ -0,0 +1,180 @@
+package com.chameleonvision.vision;
+
+import com.chameleonvision.config.CameraConfig;
+import com.chameleonvision.config.CameraJsonConfig;
+import com.chameleonvision.config.ConfigManager;
+import com.chameleonvision.config.FullCameraConfiguration;
+import com.chameleonvision.util.Helpers;
+import com.chameleonvision.util.Platform;
+import com.chameleonvision.vision.camera.CameraCapture;
+import com.chameleonvision.vision.camera.USBCameraCapture;
+import com.chameleonvision.vision.pipeline.CVPipelineSettings;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.UsbCameraInfo;
+import org.opencv.videoio.VideoCapture;
+
+import java.util.*;
+import java.util.stream.Collectors;
+
+public class VisionManager {
+    private VisionManager() {
+    }
+
+    private static final LinkedHashMap<String, UsbCameraInfo> usbCameraInfosByCameraName = new LinkedHashMap<>();
+    private static final LinkedList<FullCameraConfiguration> loadedCameraConfigs = new LinkedList<>();
+    private static final LinkedList<VisionProcessManageable> visionProcesses = new LinkedList<>();
+
+    @SuppressWarnings("WeakerAccess")
+    private static class VisionProcessManageable {
+        public final int index;
+        public final String name;
+        public final VisionProcess visionProcess;
+
+        public VisionProcessManageable(int index, String name, VisionProcess visionProcess) {
+            this.index = index;
+            this.name = name;
+            this.visionProcess = visionProcess;
+        }
+    }
+
+    private static VisionProcess currentUIVisionProcess;
+
+    public static boolean initializeSources() {
+        int suffix = 0;
+        for (UsbCameraInfo info : UsbCamera.enumerateUsbCameras()) {
+            VideoCapture cap = new VideoCapture(info.dev);
+            if (cap.isOpened()) {
+                cap.release();
+                String name = info.name;
+                while (usbCameraInfosByCameraName.containsKey(name)) {
+                    suffix++;
+                    name = String.format("%s (%d)", name, suffix);
+                }
+                usbCameraInfosByCameraName.put(name, info);
+            }
+        }
+
+        if (usbCameraInfosByCameraName.isEmpty()) {
+            return false;
+        }
+
+        // load the config
+        List<CameraJsonConfig> preliminaryConfigs = new ArrayList<>();
+
+        usbCameraInfosByCameraName.values().forEach((cameraInfo) -> {
+            String truePath;
+
+            if (Platform.CurrentPlatform.isWindows()) {
+                truePath = cameraInfo.path;
+            } else {
+                truePath = Arrays.stream(cameraInfo.otherPaths).filter(x -> x.contains("/dev/v4l/by-path")).findFirst().orElse(cameraInfo.path);
+            }
+
+            preliminaryConfigs.add(new CameraJsonConfig(truePath, cameraInfo.name));
+        });
+
+        loadedCameraConfigs.addAll(ConfigManager.initializeCameras(preliminaryConfigs));
+
+        return true;
+    }
+
+    public static boolean initializeProcesses() {
+        for (int i = 0; i < loadedCameraConfigs.size(); i++) {
+            FullCameraConfiguration config = loadedCameraConfigs.get(i);
+
+            CameraJsonConfig cameraJsonConfig = config.cameraConfig;
+
+            CameraCapture camera = new USBCameraCapture(cameraJsonConfig);
+            VisionProcess process = new VisionProcess(camera, cameraJsonConfig.name, config.pipelines);
+            process.pipelineManager.driverModePipeline.settings = config.drivermode;
+            visionProcesses.add(new VisionProcessManageable(i, cameraJsonConfig.name, process));
+        }
+        currentUIVisionProcess = getVisionProcessByIndex(0);
+        ConfigManager.settings.currentCamera = visionProcesses.get(0).name;
+        return true;
+    }
+
+    public static void startProcesses() {
+        visionProcesses.forEach((vpm) -> vpm.visionProcess.start());
+    }
+
+    public static VisionProcess getCurrentUIVisionProcess() {
+        return currentUIVisionProcess;
+    }
+
+    public static CameraConfig getCameraConfig(VisionProcess process) {
+        String cameraName = process.getCamera().getProperties().name;
+        return Objects.requireNonNull(loadedCameraConfigs.stream().filter(x -> x.cameraConfig.name.equals(cameraName)).findFirst().orElse(null)).fileConfig;
+    }
+
+    public static void setCurrentProcessByIndex(int processIndex) {
+        if (processIndex > visionProcesses.size() - 1) {
+            return;
+        }
+
+        currentUIVisionProcess = getVisionProcessByIndex(processIndex);
+        ConfigManager.settings.currentCamera = visionProcesses.get(processIndex).name;
+    }
+
+    public static VisionProcess getVisionProcessByIndex(int processIndex) {
+        if (processIndex > visionProcesses.size() - 1) {
+            return null;
+        }
+
+        VisionProcessManageable vpm =  visionProcesses.stream().filter(manageable -> manageable.index == processIndex).findFirst().orElse(null);
+        return vpm != null ? vpm.visionProcess : null;
+    }
+
+    public static List<String> getAllCameraNicknames() {
+        return visionProcesses.stream().map(vpm -> vpm.visionProcess.getCamera()
+                .getProperties().getNickname()).collect(Collectors.toList());
+    }
+
+    public static List<String> getCurrentCameraPipelineNicknames() {
+        return currentUIVisionProcess.pipelineManager.pipelines.stream().map(cvPipeline -> cvPipeline.settings.nickname).collect(Collectors.toList());
+    }
+
+
+    public static void saveAllCameras() {
+        visionProcesses.forEach((vpm) -> {
+            VisionProcess process = vpm.visionProcess;
+            String cameraName = process.getCamera().getProperties().name;
+            List<CVPipelineSettings> pipelines = process.pipelineManager.pipelines.stream().map(cvPipeline -> cvPipeline.settings).collect(Collectors.toList());
+            CVPipelineSettings driverMode = process.getDriverModeSettings();
+            CameraJsonConfig config = CameraJsonConfig.fromVisionProcess(process);
+            ConfigManager.saveCameraPipelines(cameraName, pipelines);
+            ConfigManager.saveCameraDriverMode(cameraName, driverMode);
+            ConfigManager.saveCameraConfig(cameraName, config);
+        });
+    }
+
+    private static String getCurrentCameraName() {
+        return currentUIVisionProcess.getCamera().getProperties().name;
+    }
+
+    public static void saveCurrentCameraSettings() {
+        CameraJsonConfig config = CameraJsonConfig.fromVisionProcess(currentUIVisionProcess);
+        ConfigManager.saveCameraConfig(getCurrentCameraName(), config);
+    }
+
+    public static void saveCurrentCameraPipelines() {
+        currentUIVisionProcess.pipelineManager.saveAllPipelines();
+    }
+
+    public static void saveCurrentCameraDriverMode() {
+        currentUIVisionProcess.pipelineManager.saveDriverModeConfig();
+    }
+
+    private static List<HashMap> getCameraResolutionList(CameraCapture capture) {
+        return capture.getProperties().getVideoModes().stream().map(Helpers::VideoModeToHashMap).collect(Collectors.toList());
+    }
+
+    public static List<HashMap> getCurrentCameraResolutionList() {
+        return getCameraResolutionList(currentUIVisionProcess.getCamera());
+    }
+
+    public static int getCurrentUIVisionProcessIndex() {
+        VisionProcessManageable vpm = visionProcesses.stream().filter(v -> v.visionProcess == currentUIVisionProcess).findFirst().orElse(null);
+        return vpm != null ? vpm.index : -1;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java

@@ -0,0 +1,303 @@
+package com.chameleonvision.vision;
+
+import com.chameleonvision.Debug;
+import com.chameleonvision.config.ConfigManager;
+import com.chameleonvision.util.LoopingRunnable;
+import com.chameleonvision.vision.camera.CameraCapture;
+import com.chameleonvision.vision.camera.CameraStreamer;
+import com.chameleonvision.vision.pipeline.*;
+import com.chameleonvision.web.SocketHandler;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.networktables.*;
+import edu.wpi.first.wpiutil.CircularBuffer;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.concurrent.BlockingQueue;
+import java.util.concurrent.LinkedBlockingDeque;
+
+
+public class VisionProcess {
+
+    private final CameraCapture cameraCapture;
+    private final CameraStreamerRunnable streamRunnable;
+    private final VisionProcessRunnable visionRunnable;
+    public final CameraStreamer cameraStreamer;
+    public PipelineManager pipelineManager;
+
+    private volatile CVPipelineResult lastPipelineResult;
+
+    private BlockingQueue<Mat> streamFrameQueue = new LinkedBlockingDeque<>(1);
+
+    // network table stuff
+    private final NetworkTable defaultTable;
+    private NetworkTableEntry ntPipelineEntry;
+    public NetworkTableEntry ntDriverModeEntry;
+    private int ntDriveModeListenerID;
+    private int ntPipelineListenerID;
+    private NetworkTableEntry ntYawEntry;
+    private NetworkTableEntry ntPitchEntry;
+    private NetworkTableEntry ntAuxListEntry;
+    private NetworkTableEntry ntAreaEntry;
+    private NetworkTableEntry ntTimeStampEntry;
+    private NetworkTableEntry ntValidEntry;
+    private ObjectMapper objectMapper = new ObjectMapper();
+
+    VisionProcess(CameraCapture cameraCapture, String name, List<CVPipelineSettings> loadedPipelineSettings) {
+        this.cameraCapture = cameraCapture;
+
+        pipelineManager = new PipelineManager(this, loadedPipelineSettings);
+
+        // Thread to put frames on the dashboard
+        this.cameraStreamer = new CameraStreamer(cameraCapture, name);
+        this.streamRunnable = new CameraStreamerRunnable(30, cameraStreamer);
+
+        // Thread to process vision data
+        this.visionRunnable = new VisionProcessRunnable();
+
+        // network table
+        defaultTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraCapture.getProperties().getNickname());
+    }
+
+    public void start() {
+        System.out.println("Starting NetworkTables.");
+        initNT(defaultTable);
+
+        System.out.println("Starting vision thread.");
+        var visionThread = new Thread(visionRunnable);
+        visionThread.setName(getCamera().getProperties().name + " - Vision Thread");
+        visionThread.start();
+
+        System.out.println("Starting stream thread.");
+        var streamThread = new Thread(streamRunnable);
+        streamThread.setName(getCamera().getProperties().name + " - Stream Thread");
+        streamThread.start();
+    }
+
+    /**
+     * Removes the old value change listeners
+     * calls {@link #initNT}
+     *
+     * @param newTable passed to {@link #initNT}
+     */
+    public void resetNT(NetworkTable newTable) {
+        ntDriverModeEntry.removeListener(ntDriveModeListenerID);
+        ntPipelineEntry.removeListener(ntPipelineListenerID);
+        initNT(newTable);
+    }
+
+    public void setCameraNickname(String newName) {
+        getCamera().getProperties().setNickname(newName);
+        var newTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + newName);
+        resetNT(newTable);
+    }
+
+    private void initNT(NetworkTable newTable) {
+        ntPipelineEntry = newTable.getEntry("pipeline");
+        ntDriverModeEntry = newTable.getEntry("driver_mode");
+        ntPitchEntry = newTable.getEntry("pitch");
+        ntYawEntry = newTable.getEntry("yaw");
+        ntAreaEntry = newTable.getEntry("area");
+        ntTimeStampEntry = newTable.getEntry("timestamp");
+        ntValidEntry = newTable.getEntry("is_valid");
+        ntAuxListEntry = newTable.getEntry("aux_targets");
+        ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate);
+        ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate);
+        ntDriverModeEntry.setBoolean(false);
+        ntPipelineEntry.setNumber(pipelineManager.getCurrentPipelineIndex());
+        pipelineManager.ntIndexEntry = ntPipelineEntry;
+    }
+
+    private void setDriverMode(EntryNotification driverModeEntryNotification) {
+        setDriverMode(driverModeEntryNotification.value.getBoolean());
+    }
+
+    public void setDriverMode(boolean driverMode) {
+        pipelineManager.setDriverMode(driverMode);
+        SocketHandler.sendFullSettings();
+    }
+
+    /**
+     * Method called by the nt entry listener to update the next pipeline.
+     * @param notification the notification
+     */
+    private void setPipeline(EntryNotification notification) {
+        var wantedPipelineIndex = (int) notification.value.getDouble();
+        pipelineManager.setCurrentPipeline(wantedPipelineIndex);
+    }
+
+    public void setDriverModeEntry(boolean isDriverMode) {
+
+        // if it's null, we haven't even started the program yet, so just return
+        // otherwise, set it.
+        if(ntDriverModeEntry != null) {
+            ntDriverModeEntry.setBoolean(isDriverMode);
+        }
+    }
+
+    private void updateUI(CVPipelineResult data) {
+        if(cameraCapture.getProperties().name.equals(ConfigManager.settings.currentCamera)) {
+            HashMap<String, Object> WebSend = new HashMap<>();
+            HashMap<String, Object> point = new HashMap<>();
+            HashMap<String, Object> calculated = new HashMap<>();
+            List<Double> center = new ArrayList<>();
+            if (data.hasTarget) {
+                if(data instanceof CVPipeline2d.CVPipeline2dResult) {
+                    CVPipeline2d.CVPipeline2dResult result = (CVPipeline2d.CVPipeline2dResult) data;
+                    CVPipeline2d.Target2d bestTarget = result.targets.get(0);
+                    center.add(bestTarget.rawPoint.center.x);
+                    center.add(bestTarget.rawPoint.center.y);
+                    calculated.put("pitch", bestTarget.pitch);
+                    calculated.put("yaw", bestTarget.yaw);
+                    calculated.put("area", bestTarget.area);
+                } else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
+                    // TODO: (2.1) 3d stuff in UI
+                } else {
+                    center.add(0.0);
+                    center.add(0.0);
+                    calculated.put("pitch", 0);
+                    calculated.put("yaw", 0);
+                }
+            } else {
+                center.add(0.0);
+                center.add(0.0);
+                calculated.put("pitch", 0);
+                calculated.put("yaw", 0);
+            }
+            point.put("fps", visionRunnable.fps);
+            point.put("calculated", calculated);
+            point.put("rawPoint", center);
+            WebSend.put("point", point);
+            SocketHandler.broadcastMessage(WebSend);
+        }
+    }
+
+    private void updateNetworkTableData(CVPipelineResult data) {
+        ntValidEntry.setBoolean(data.hasTarget);
+        if(data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) {
+            if(data instanceof CVPipeline2d.CVPipeline2dResult) {
+
+                //noinspection unchecked
+                List<CVPipeline2d.Target2d> targets = (List<CVPipeline2d.Target2d>) data.targets;
+                ntTimeStampEntry.setDouble(data.imageTimestamp);
+                ntPitchEntry.setDouble(targets.get(0).pitch);
+                ntYawEntry.setDouble(targets.get(0).yaw);
+                ntAreaEntry.setDouble(targets.get(0).area);
+                try {
+                    ntAuxListEntry.setString(objectMapper.writeValueAsString(targets));
+                } catch (JsonProcessingException e) {
+                    e.printStackTrace();
+                }
+            } else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
+                // TODO: (2.1) 3d stuff...
+            }
+        } else {
+            ntPitchEntry.setDouble(0.0);
+            ntYawEntry.setDouble(0.0);
+            ntAreaEntry.setDouble(0.0);
+            ntTimeStampEntry.setDouble(0.0);
+            ntAuxListEntry.setString("");
+        }
+    }
+
+    public void setVideoMode(VideoMode newMode) {
+        cameraCapture.setVideoMode(newMode);
+        cameraStreamer.setNewVideoMode(newMode);
+    }
+
+    public VideoMode getCurrentVideoMode() {
+        return cameraCapture.getCurrentVideoMode();
+    }
+
+    public List<VideoMode> getPossibleVideoModes() {
+        return cameraCapture.getProperties().videoModes;
+    }
+
+    public CameraCapture getCamera() {
+        return cameraCapture;
+    }
+
+    public CVPipelineSettings getDriverModeSettings() {
+        return pipelineManager.driverModePipeline.settings;
+    }
+
+    /**
+     * VisionProcessRunnable will process images as quickly as possible
+     */
+    private class VisionProcessRunnable implements Runnable {
+
+        volatile Double fps = 0.0;
+        private CircularBuffer fpsAveragingBuffer = new CircularBuffer(7);
+
+        @Override
+        public void run() {
+            var lastUpdateTimeNanos = System.nanoTime();
+            while(!Thread.interrupted()) {
+
+                // blocking call, will block until camera has a new frame.
+                Pair<Mat, Long> camData = cameraCapture.getFrame();
+
+                Mat camFrame = camData.getLeft();
+                if (camFrame.cols() > 0 && camFrame.rows() > 0) {
+                    CVPipelineResult result = pipelineManager.getCurrentPipeline().runPipeline(camFrame);
+
+                    if (result != null) {
+                        result.setTimestamp(camData.getRight());
+                        lastPipelineResult = result;
+                        updateNetworkTableData(lastPipelineResult);
+                        updateUI(lastPipelineResult);
+                    }
+                }
+
+                try {
+                    streamFrameQueue.clear();
+                    streamFrameQueue.add(lastPipelineResult.outputMat);
+                } catch (Exception e) {
+                    Debug.printInfo("Vision running faster than stream.");
+                }
+
+                var deltaTimeNanos = System.nanoTime() - lastUpdateTimeNanos;
+                fpsAveragingBuffer.addFirst(1.0 / (deltaTimeNanos * 1E-09));
+                lastUpdateTimeNanos = System.nanoTime();
+                fps = getAverageFPS();
+            }
+        }
+
+        double getAverageFPS() {
+            var temp = 0.0;
+            for(int i = 0; i < 7; i++) {
+                temp += fpsAveragingBuffer.get(i);
+            }
+            temp /= 7.0;
+            return temp;
+        }
+
+    }
+
+    private class CameraStreamerRunnable extends LoopingRunnable {
+
+        final CameraStreamer streamer;
+
+        private CameraStreamerRunnable(int cameraFPS, CameraStreamer streamer) {
+            // add 2 FPS to allow for a bit of overhead
+            super(1000L/(cameraFPS + 2));
+            this.streamer = streamer;
+        }
+
+        @Override
+        protected void process() {
+            if (!streamFrameQueue.isEmpty()) {
+                try {
+                    streamer.runStream(streamFrameQueue.take());
+                } catch (InterruptedException e) {
+                    e.printStackTrace();
+                }
+            }
+        }
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/camera/CameraCapture.java

@@ -0,0 +1,41 @@
+package com.chameleonvision.vision.camera;
+
+import com.chameleonvision.vision.image.ImageCapture;
+import edu.wpi.cscore.VideoMode;
+
+public interface CameraCapture extends ImageCapture {
+    USBCameraProperties getProperties();
+
+    VideoMode getCurrentVideoMode();
+
+    /**
+     * Set the exposure of the camera
+     * @param exposure the new exposure to set the camera to
+     */
+    void setExposure(int exposure);
+
+    /**
+     * Set the brightness of the camera
+     * @param brightness the new brightness to set the camera to
+     */
+    void setBrightness(int brightness);
+
+    /**
+     * Set the video mode (fps and resolution) of the camera
+     * @param mode the desired mode
+     */
+    void setVideoMode(VideoMode mode);
+
+    /**
+     * Set the video mode (fps and resolution) of the camera
+     * @param index the index of the desired mode
+     */
+    void setVideoMode(int index);
+
+    /**
+     * Set the gain of the camera
+     * NOTE - Not all cameras support this.
+     * @param gain the new gain to set the camera to
+     */
+    void setGain(int gain);
+}

chameleon-server/src/main/java/com/chameleonvision/vision/camera/CameraStreamer.java

@@ -0,0 +1,85 @@
+package com.chameleonvision.vision.camera;
+
+import com.chameleonvision.vision.enums.StreamDivisor;
+import com.chameleonvision.web.SocketHandler;
+import edu.wpi.cscore.CvSource;
+import edu.wpi.cscore.MjpegServer;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.cameraserver.CameraServer;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+public class CameraStreamer {
+    private final CameraCapture cameraCapture;
+    private final String name;
+    private StreamDivisor divisor = StreamDivisor.NONE;
+    private CvSource cvSource;
+    private final Object streamBufferLock = new Object();
+    private Mat streamBuffer = new Mat();
+    private Size size;
+
+    public CameraStreamer(CameraCapture cameraCapture, String name) {
+        this.cameraCapture = cameraCapture;
+        this.name = name;
+        this.cvSource = CameraServer.getInstance().putVideo(name,
+                cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value,
+                cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value);
+        //noinspection IntegerDivisionInFloatingPointContext
+        this.size = new Size(
+                cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value,
+                cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value
+        );
+        setDivisor(divisor, false);
+    }
+
+    public void setDivisor(StreamDivisor newDivisor, boolean updateUI) {
+        if (divisor != newDivisor) {
+            this.divisor = newDivisor;
+            var camValues = cameraCapture.getProperties();
+            var newWidth = camValues.getStaticProperties().imageWidth / newDivisor.value;
+            var newHeight = camValues.getStaticProperties().imageHeight / newDivisor.value;
+            this.size = new Size(newWidth, newHeight);
+            synchronized (streamBufferLock) {
+                this.streamBuffer = new Mat(newWidth, newHeight, CvType.CV_8UC3);
+                this.cvSource = CameraServer.getInstance().putVideo(this.name,
+                        cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value,
+                        cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value);
+            }
+            if (updateUI) {
+                SocketHandler.sendFullSettings();
+            }
+        }
+    }
+
+    public StreamDivisor getDivisor() {
+        return divisor;
+    }
+
+    public void setNewVideoMode(VideoMode newVideoMode) {
+        // Trick to update cvSource and streamBuffer to the new resolution
+        // Must change the cameraProcess resolution first
+        setDivisor(divisor, true);
+    }
+
+    public int getStreamPort() {
+        var s = (MjpegServer) CameraServer.getInstance().getServer("serve_" + name);
+        return s.getPort();
+    }
+
+    public void runStream(Mat image) {
+        synchronized (streamBufferLock) {
+            image.copyTo(streamBuffer);
+        }
+
+        if (divisor.value != 1) {
+//            var camVal = cameraProcess.getProperties().staticProperties;
+//            var newWidth = camVal.imageWidth / divisor.value;
+//            var newHeight = camVal.imageHeight / divisor.value;
+//            Size newSize = new Size(newWidth, newHeight);
+             Imgproc.resize(streamBuffer, streamBuffer, this.size);
+        }
+        cvSource.putFrame(streamBuffer);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/camera/CaptureStaticProperties.java

@@ -0,0 +1,39 @@
+package com.chameleonvision.vision.camera;
+
+import edu.wpi.cscore.VideoMode;
+import org.apache.commons.math3.fraction.Fraction;
+import org.apache.commons.math3.util.FastMath;
+
+public class CaptureStaticProperties {
+
+    public final int imageWidth;
+    public final int imageHeight;
+    public final double fov;
+    public final double imageArea;
+    public final double centerX;
+    public final double centerY;
+    public final double horizontalFocalLength;
+    public final double verticalFocalLength;
+    public final VideoMode mode;
+
+    public CaptureStaticProperties(VideoMode mode, int imageWidth, int imageHeight, double fov) {
+        this.mode = mode;
+        this.imageWidth = imageWidth;
+        this.imageHeight = imageHeight;
+        this.fov = fov;
+        imageArea = this.imageWidth * this.imageHeight;
+        centerX = ((double) this.imageWidth / 2) - 0.5;
+        centerY = ((double) this.imageHeight / 2) - 0.5;
+
+        // pinhole model calculations
+        double diagonalView = FastMath.toRadians(this.fov);
+        Fraction aspectFraction = new Fraction(this.imageWidth, this.imageHeight);
+        int horizontalRatio = aspectFraction.getNumerator();
+        int verticalRatio = aspectFraction.getDenominator();
+        double diagonalAspect = FastMath.hypot(horizontalRatio, verticalRatio);
+        double horizontalView = FastMath.atan(FastMath.tan(diagonalView / 2) * (horizontalRatio / diagonalAspect)) * 2;
+        double verticalView = FastMath.atan(FastMath.tan(diagonalView / 2) * (verticalRatio / diagonalAspect)) * 2;
+        horizontalFocalLength = this.imageWidth / (2 * FastMath.tan(horizontalView /2));
+        verticalFocalLength = this.imageHeight / (2 * FastMath.tan(verticalView /2));
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/camera/USBCameraCapture.java

@@ -0,0 +1,89 @@
+package com.chameleonvision.vision.camera;
+
+import com.chameleonvision.config.CameraJsonConfig;
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.VideoException;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.cameraserver.CameraServer;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+
+public class USBCameraCapture implements CameraCapture {
+    private final UsbCamera baseCamera;
+    private final CvSink cvSink;
+    private Mat imageBuffer = new Mat();
+    private USBCameraProperties properties;
+
+    public USBCameraCapture(CameraJsonConfig config) {
+        baseCamera = new UsbCamera(config.name, config.path);
+        cvSink = CameraServer.getInstance().getVideo(baseCamera);
+        properties = new USBCameraProperties(baseCamera, config);
+
+        int videoMode = properties.videoModes.size() - 1 <= config.videomode ? config.videomode : 0;
+        setVideoMode(videoMode);
+    }
+
+    @Override
+    public USBCameraProperties getProperties() {
+        return properties;
+    }
+
+    @Override
+    public VideoMode getCurrentVideoMode() {
+        return baseCamera.getVideoMode();
+    }
+
+    @Override
+    public Pair<Mat, Long> getFrame() {
+        Long deltaTime;
+        // TODO: Why multiply by 1000 here?
+        deltaTime = cvSink.grabFrame(imageBuffer) * 1000L;
+        return Pair.of(imageBuffer, deltaTime);
+    }
+
+    @Override
+    public void setExposure(int exposure) {
+        try {
+            baseCamera.setExposureManual(exposure);
+        } catch (VideoException e) {
+            System.err.println("Failed to change camera exposure!");
+        }
+    }
+
+    @Override
+    public void setBrightness(int brightness) {
+        try {
+            baseCamera.setBrightness(brightness);
+        } catch (VideoException e) {
+            System.err.println("Failed to change camera brightness!");
+        }
+    }
+
+    @Override
+    public void setVideoMode(VideoMode mode) {
+        try {
+            baseCamera.setVideoMode(mode);
+            properties.updateVideoMode(mode);
+        } catch (VideoException e) {
+            System.err.println("Failed to change camera video mode!");
+        }
+    }
+
+    public void setVideoMode(int index){
+        VideoMode mode = properties.getVideoModes().get(index);
+        setVideoMode(mode);
+    }
+
+    @Override
+    public void setGain(int gain) {
+        if (properties.isPS3Eye) {
+            try {
+                baseCamera.getProperty("gain_automatic").set(0);
+                baseCamera.getProperty("gain").set(gain);
+            } catch (Exception e) {
+                System.err.println("Failed to change camera gain!");
+            }
+        }
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/camera/USBCameraProperties.java

@@ -0,0 +1,109 @@
+package com.chameleonvision.vision.camera;
+
+import com.chameleonvision.config.CameraJsonConfig;
+import com.chameleonvision.util.Platform;
+import com.chameleonvision.vision.image.CaptureProperties;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.VideoMode;
+
+import java.util.Arrays;
+import java.util.List;
+import java.util.function.Predicate;
+import java.util.stream.Collectors;
+import java.util.stream.Stream;
+
+public class USBCameraProperties extends CaptureProperties {
+    public static final double DEFAULT_FOV = 70;
+    private static final int DEFAULT_EXPOSURE = 50;
+    private static final int DEFAULT_BRIGHTNESS = 50;
+    private static final int MINIMUM_FPS = 30;
+    private static final int MINIMUM_WIDTH = 320;
+    private static final int MINIMUM_HEIGHT = 200;
+    private static final int MAX_INIT_MS = 1500;
+
+    private static final int PS3EYE_VID = 1415;
+    private static final int PS3EYE_PID = 2000;
+
+    private static final List<VideoMode.PixelFormat> ALLOWED_PIXEL_FORMATS = Arrays.asList(VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG);
+
+    private static final Predicate<VideoMode> kMinFPSPredicate = (videoMode -> videoMode.fps >= MINIMUM_FPS);
+    private static final Predicate<VideoMode> kMinSizePredicate = (videoMode -> videoMode.width >= MINIMUM_WIDTH && videoMode.height >= MINIMUM_HEIGHT);
+    private static final Predicate<VideoMode> kPixelFormatPredicate = (videoMode -> ALLOWED_PIXEL_FORMATS.contains(videoMode.pixelFormat));
+
+    public final String name;
+    public final String path;
+    public final List<VideoMode> videoModes;
+
+    private final UsbCamera baseCamera;
+    public final boolean isPS3Eye;
+
+    private String nickname;
+    private double FOV;
+
+    USBCameraProperties(UsbCamera baseCamera, CameraJsonConfig config) {
+        FOV = config.fov;
+        name = config.name;
+        path = config.path;
+        nickname = config.nickname;
+        this.baseCamera = baseCamera;
+
+        int usbVID = baseCamera.getInfo().vendorId;
+        int usbPID = baseCamera.getInfo().productId;
+
+        // wait for camera USB init on Windows, Windows USB is slow...
+        if (Platform.CurrentPlatform == Platform.WINDOWS_64 && !baseCamera.isConnected()) {
+            System.out.print("Waiting on camera... ");
+            long initTimeout = System.nanoTime();
+            while (!baseCamera.isConnected()) {
+                if (((System.nanoTime() - initTimeout) / 1e6) >= MAX_INIT_MS) {
+                    break;
+                }
+            }
+            var initTimeMs = (System.nanoTime() - initTimeout) / 1e6;
+            System.out.printf("USBCameraProcess initialized in %.2fms\n", initTimeMs);
+        }
+
+        isPS3Eye = (usbVID == PS3EYE_VID && usbPID == PS3EYE_PID);
+        videoModes = filterVideoModes(baseCamera.enumerateVideoModes());
+    }
+
+    public void setFOV(double FOV) {
+        if (this.FOV != FOV) {
+            this.FOV = FOV;
+            staticProperties = new CaptureStaticProperties(staticProperties.mode, staticProperties.imageWidth, staticProperties.imageHeight, FOV);
+        }
+    }
+
+    public double getFOV() {
+        return FOV;
+    }
+
+    public void setNickname(String nickname) {
+        this.nickname = nickname;
+    }
+
+    public String getNickname() {
+        return nickname;
+    }
+
+    private List<VideoMode> filterVideoModes(VideoMode[] videoModes) {
+        Predicate<VideoMode> fullPredicate = kMinFPSPredicate.and(kMinSizePredicate).and(kPixelFormatPredicate);
+        Stream<VideoMode> validModes = Arrays.stream(videoModes).filter(fullPredicate);
+        return validModes.collect(Collectors.toList());
+    }
+
+    void updateVideoMode(VideoMode videoMode) {
+        staticProperties = new CaptureStaticProperties(videoMode, videoMode.width, videoMode.height, FOV);
+    }
+
+    public List<VideoMode> getVideoModes() {
+        return videoModes;
+    }
+
+    public VideoMode getCurrentVideoMode() { return staticProperties.mode; }
+
+    public int getCurrentVideoModeIndex(){
+        return getVideoModes().indexOf(getCurrentVideoMode());
+    }
+
+}

chameleon-server/src/main/java/com/chameleonvision/vision/enums/CalibrationMode.java

@@ -0,0 +1,5 @@
+package com.chameleonvision.vision.enums;
+
+public enum CalibrationMode {
+    None,Single,Dual
+}

chameleon-server/src/main/java/com/chameleonvision/vision/enums/ImageFlipMode.java

@@ -0,0 +1,14 @@
+package com.chameleonvision.vision.enums;
+
+public enum ImageFlipMode {
+    NONE(Integer.MIN_VALUE),
+    VERTICAL(1),
+    HORIZONTAL(0),
+    BOTH(-1);
+
+    public final int value;
+
+    ImageFlipMode(int value) {
+        this.value = value;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/enums/ImageRotationMode.java

@@ -0,0 +1,16 @@
+package com.chameleonvision.vision.enums;
+
+import org.opencv.core.Core;
+
+public enum ImageRotationMode {
+    DEG_0(-1),
+    DEG_90(Core.ROTATE_90_CLOCKWISE),
+    DEG_180(Core.ROTATE_180),
+    DEG_270(Core.ROTATE_90_COUNTERCLOCKWISE);
+
+    public final int value;
+
+    ImageRotationMode(int value) {
+        this.value = value;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/enums/SortMode.java

@@ -0,0 +1,5 @@
+package com.chameleonvision.vision.enums;
+
+public enum SortMode {
+    Largest,Smallest,Highest,Lowest,Rightmost,Leftmost,Centermost
+}

chameleon-server/src/main/java/com/chameleonvision/vision/enums/StreamDivisor.java

@@ -0,0 +1,14 @@
+package com.chameleonvision.vision.enums;
+
+public enum StreamDivisor {
+    NONE(1),
+    HALF(2),
+    QUARTER(4),
+    SIXTH(6);
+
+    public final Integer value;
+
+    StreamDivisor(int value) {
+        this.value = value;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/enums/TargetGroup.java

@@ -0,0 +1,6 @@
+package com.chameleonvision.vision.enums;
+
+public enum TargetGroup {
+    Single,
+    Dual
+}

chameleon-server/src/main/java/com/chameleonvision/vision/enums/TargetIntersection.java

@@ -0,0 +1,5 @@
+package com.chameleonvision.vision.enums;
+
+public enum TargetIntersection {
+    None,Up,Down,Left,Right
+}

chameleon-server/src/main/java/com/chameleonvision/vision/image/CaptureProperties.java

@@ -0,0 +1,21 @@
+package com.chameleonvision.vision.image;
+
+import com.chameleonvision.vision.camera.CaptureStaticProperties;
+import edu.wpi.cscore.VideoMode;
+import org.opencv.core.Mat;
+
+public class CaptureProperties {
+
+    protected CaptureStaticProperties staticProperties;
+
+    protected CaptureProperties() {
+    }
+
+    public CaptureProperties(Mat staticImage, double fov) {
+        staticProperties = new CaptureStaticProperties(new VideoMode(0, staticImage.cols(), staticImage.rows(), 99999), staticImage.cols(), staticImage.rows(), fov);
+    }
+
+    public CaptureStaticProperties getStaticProperties() {
+        return staticProperties;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/image/ImageCapture.java

@@ -0,0 +1,12 @@
+package com.chameleonvision.vision.image;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+
+public interface ImageCapture {
+    /**
+     * Get the next camera frame
+     * @return a Pair of the captured image and the Linux epoch of when the frame was grabbed (in uS)
+     */
+    Pair<Mat, Long> getFrame();
+}

chameleon-server/src/main/java/com/chameleonvision/vision/image/StaticImageCapture.java

@@ -0,0 +1,32 @@
+package com.chameleonvision.vision.image;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.imgcodecs.Imgcodecs;
+
+import java.nio.file.Files;
+import java.nio.file.Path;
+
+public class StaticImageCapture implements ImageCapture {
+
+    private final Mat image = new Mat();
+
+    public StaticImageCapture(Path imagePath) {
+        if (!Files.exists(imagePath)) throw new RuntimeException("Invalid path for image!");
+
+        Mat tempMat = new Mat();
+
+        try {
+            tempMat = Imgcodecs.imread(imagePath.toString());
+        } catch (Exception e) {
+            System.err.println("Failed to read image!");
+        } finally {
+            tempMat.copyTo(image);
+        }
+    }
+
+    @Override
+    public Pair<Mat, Long> getFrame() {
+        return Pair.of(image, System.nanoTime());
+    }
+}

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

@@ -0,0 +1,35 @@
+package com.chameleonvision.vision.pipeline;
+
+import com.chameleonvision.vision.camera.CameraCapture;
+import org.opencv.core.Mat;
+
+/**
+ *
+ * @param <R> Pipeline result type
+ */
+public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings> {
+    protected Mat outputMat = new Mat();
+    CameraCapture cameraCapture;
+    public S settings;
+
+    protected CVPipeline(S settings) {
+        this.settings = settings;
+    }
+
+    protected CVPipeline(String pipelineName, S settings) {
+        this.settings = settings;
+        settings.nickname = pipelineName;
+    }
+
+    public void initPipeline(CameraCapture camera) {
+        cameraCapture = camera;
+        cameraCapture.setExposure((int) settings.exposure);
+        cameraCapture.setBrightness((int) settings.brightness);
+        cameraCapture.setGain((int) settings.gain);
+    }
+    abstract public R runPipeline(Mat inputMat);
+
+    public boolean is3D() {
+        return (this instanceof CVPipeline3d);
+    }
+}

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

@@ -0,0 +1,201 @@
+package com.chameleonvision.vision.pipeline;
+
+import com.chameleonvision.Main;
+import com.chameleonvision.vision.camera.CameraCapture;
+import com.chameleonvision.vision.camera.CaptureStaticProperties;
+import com.chameleonvision.vision.pipeline.pipes.*;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.*;
+
+import java.util.List;
+
+import static com.chameleonvision.vision.pipeline.CVPipeline2d.*;
+
+@SuppressWarnings("WeakerAccess")
+public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSettings> {
+
+    private Mat rawCameraMat = new Mat();
+
+    private RotateFlipPipe rotateFlipPipe;
+    private BlurPipe blurPipe;
+    private ErodeDilatePipe erodeDilatePipe;
+    private HsvPipe hsvPipe;
+    private FindContoursPipe findContoursPipe;
+    private FilterContoursPipe filterContoursPipe;
+    private SpeckleRejectPipe speckleRejectPipe;
+    private GroupContoursPipe groupContoursPipe;
+    private SortContoursPipe sortContoursPipe;
+    private Collect2dTargetsPipe collect2dTargetsPipe;
+    private Draw2dContoursPipe.Draw2dContoursSettings draw2dContoursSettings;
+    private Draw2dContoursPipe draw2dContoursPipe;
+    private OutputMatPipe outputMatPipe;
+
+    private String pipelineTimeString = "";
+    private CaptureStaticProperties camProps;
+    private Scalar hsvLower, hsvUpper;
+
+    public CVPipeline2d() {
+        super(new CVPipeline2dSettings());
+    }
+
+    public CVPipeline2d(String name) {
+        super(name, new CVPipeline2dSettings());
+    }
+
+    public CVPipeline2d(CVPipeline2dSettings settings) {
+        super(settings);
+    }
+
+    @Override
+    public void initPipeline(CameraCapture process) {
+        super.initPipeline(process);
+
+        camProps = cameraCapture.getProperties().getStaticProperties();
+        hsvLower = new Scalar(settings.hue.get(0).intValue(), settings.saturation.get(0).intValue(), settings.value.get(0).intValue());
+        hsvUpper = new Scalar(settings.hue.get(1).intValue(), settings.saturation.get(1).intValue(), settings.value.get(1).intValue());
+
+        rotateFlipPipe = new RotateFlipPipe(settings.rotationMode, settings.flipMode);
+        blurPipe = new BlurPipe(5);
+        erodeDilatePipe = new ErodeDilatePipe(settings.erode, settings.dilate, 7);
+        hsvPipe = new HsvPipe(hsvLower, hsvUpper);
+        findContoursPipe = new FindContoursPipe();
+        filterContoursPipe = new FilterContoursPipe(settings.area, settings.ratio, settings.extent, camProps);
+        speckleRejectPipe = new SpeckleRejectPipe(settings.speckle.doubleValue());
+        groupContoursPipe = new GroupContoursPipe(settings.targetGroup, settings.targetIntersection);
+        sortContoursPipe = new SortContoursPipe(settings.sortMode, camProps, 5);
+        collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode, settings.point,
+                settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps);
+        draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings();
+        // TODO: make settable from UI? config?
+        draw2dContoursSettings.showCentroid = false;
+        draw2dContoursSettings.showCrosshair = true;
+        draw2dContoursSettings.boxOutlineSize = 2;
+        draw2dContoursSettings.showRotatedBox = true;
+        draw2dContoursSettings.showMaximumBox = true;
+        draw2dContoursSettings.showMultiple = settings.multiple;
+
+        draw2dContoursPipe = new Draw2dContoursPipe(draw2dContoursSettings, camProps);
+        outputMatPipe = new OutputMatPipe(settings.isBinary);
+    }
+
+    @Override
+    public CVPipeline2dResult runPipeline(Mat inputMat) {
+        long totalPipelineTimeNanos = 0;
+        long pipelineStartTimeNanos = System.nanoTime();
+
+        if (cameraCapture == null) {
+            throw new RuntimeException("Pipeline was not initialized before being run!");
+        }
+
+        // TODO (HIGH) find the source of the random NPE
+        if (settings == null) {
+            throw new RuntimeException("settings was not initialized!");
+        }
+        if (inputMat.cols() <= 1) {
+            throw new RuntimeException("Input Mat is empty!");
+        }
+
+        pipelineTimeString = "";
+
+        inputMat.copyTo(rawCameraMat);
+
+        // prepare pipes
+        camProps = cameraCapture.getProperties().getStaticProperties();
+        hsvLower = new Scalar(settings.hue.get(0).intValue(), settings.saturation.get(0).intValue(), settings.value.get(0).intValue());
+        hsvUpper = new Scalar(settings.hue.get(1).intValue(), settings.saturation.get(1).intValue(), settings.value.get(1).intValue());
+        rotateFlipPipe.setConfig(settings.rotationMode, settings.flipMode);
+        blurPipe.setConfig(0);
+        erodeDilatePipe.setConfig(settings.erode, settings.dilate, 7);
+        hsvPipe.setConfig(hsvLower, hsvUpper);
+        filterContoursPipe.setConfig(settings.area, settings.ratio, settings.extent, camProps);
+        speckleRejectPipe.setConfig(settings.speckle.doubleValue());
+        groupContoursPipe.setConfig(settings.targetGroup, settings.targetIntersection);
+        sortContoursPipe.setConfig(settings.sortMode, camProps, 5);
+        collect2dTargetsPipe.setConfig(settings.calibrationMode, settings.point,
+                settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps);
+        draw2dContoursPipe.setConfig(settings.multiple, camProps);
+        outputMatPipe.setConfig(settings.isBinary);
+
+        long pipeInitTimeNanos = System.nanoTime() - pipelineStartTimeNanos;
+
+        // run pipes
+        Pair<Mat, Long> rotateFlipResult = rotateFlipPipe.run(inputMat);
+        totalPipelineTimeNanos += rotateFlipResult.getRight();
+
+        Pair<Mat, Long> blurResult = blurPipe.run(rotateFlipResult.getLeft());
+        totalPipelineTimeNanos += blurResult.getRight();
+
+        Pair<Mat, Long> erodeDilateResult = erodeDilatePipe.run(blurResult.getLeft());
+        totalPipelineTimeNanos += erodeDilateResult.getRight();
+
+        Pair<Mat, Long> hsvResult = hsvPipe.run(erodeDilateResult.getLeft());
+        totalPipelineTimeNanos += hsvResult.getRight();
+
+        Pair<List<MatOfPoint>, Long> findContoursResult = findContoursPipe.run(hsvResult.getLeft());
+        totalPipelineTimeNanos += findContoursResult.getRight();
+
+        Pair<List<MatOfPoint>, Long> filterContoursResult = filterContoursPipe.run(findContoursResult.getLeft());
+        totalPipelineTimeNanos += filterContoursResult.getRight();
+
+        Pair<List<MatOfPoint>, Long> speckleRejectResult = speckleRejectPipe.run(filterContoursResult.getLeft());
+        totalPipelineTimeNanos += speckleRejectResult.getRight();
+
+        Pair<List<RotatedRect>, Long> groupContoursResult = groupContoursPipe.run(speckleRejectResult.getLeft());
+        totalPipelineTimeNanos += groupContoursResult.getRight();
+
+        Pair<List<RotatedRect>, Long> sortContoursResult = sortContoursPipe.run(groupContoursResult.getLeft());
+        totalPipelineTimeNanos += sortContoursResult.getRight();
+
+        Pair<List<Target2d>, Long> collect2dTargetsResult = collect2dTargetsPipe.run(Pair.of(sortContoursResult.getLeft(), camProps));
+        totalPipelineTimeNanos += collect2dTargetsResult.getRight();
+
+        // takes pair of (Mat of original camera image (8UC3), Mat of HSV thresholded image(8UC1))
+        Pair<Mat, Long> outputMatResult = outputMatPipe.run(Pair.of(rotateFlipResult.getLeft(), hsvResult.getLeft()));
+        totalPipelineTimeNanos += outputMatResult.getRight();
+
+        // takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
+        Pair<Mat, Long> draw2dContoursResult = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft()));
+        totalPipelineTimeNanos += draw2dContoursResult.getRight();
+
+        if (Main.testMode) {
+            pipelineTimeString += String.format("PipeInit: %.2fms, ", pipeInitTimeNanos / 1000000.0);
+            pipelineTimeString += String.format("RotateFlip: %.2fms, ", rotateFlipResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("Blur: %.2fms, ", blurResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("ErodeDilate: %.2fms, ", erodeDilateResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("HSV: %.2fms, ", hsvResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("FindContours: %.2fms, ", findContoursResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("FilterContours: %.2fms, ", filterContoursResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("SpeckleReject: %.2fms, ", speckleRejectResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("GroupContours: %.2fms, ", groupContoursResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("SortContours: %.2fms, ", sortContoursResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("Collect2dTargets: %.2fms, ", collect2dTargetsResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("OutputMat: %.2fms, ", outputMatResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("Draw2dContours: %.2fms, ", draw2dContoursResult.getRight() / 1000000.0);
+
+            System.out.println(pipelineTimeString);
+            double totalPipelineTimeMillis = totalPipelineTimeNanos / 1000000.0;
+            double totalPipelineTimeFPS = 1.0 / (totalPipelineTimeMillis / 1000.0);
+            double truePipelineTimeMillis = (System.nanoTime() - pipelineStartTimeNanos) / 1000000.0;
+            double truePipelineFPS = 1.0 / (truePipelineTimeMillis / 1000.0);
+            System.out.printf("Pipeline processed in %.3fms (%.2fFPS), ", totalPipelineTimeMillis, totalPipelineTimeFPS);
+            System.out.printf("full pipeline run time was %.3fms (%.2fFPS)\n", truePipelineTimeMillis, truePipelineFPS);
+        }
+
+        return new CVPipeline2dResult(collect2dTargetsResult.getLeft(), draw2dContoursResult.getLeft(), totalPipelineTimeNanos);
+    }
+
+    public static class CVPipeline2dResult extends CVPipelineResult<Target2d> {
+        public CVPipeline2dResult(List<Target2d> targets, Mat outputMat, long processTimeNanos) {
+            super(targets, outputMat, processTimeNanos);
+        }
+    }
+
+    public static class Target2d {
+        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;
+        public RotatedRect rawPoint;
+    }
+}

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

@@ -0,0 +1,30 @@
+package com.chameleonvision.vision.pipeline;
+
+import com.chameleonvision.vision.enums.CalibrationMode;
+import com.chameleonvision.vision.enums.SortMode;
+import com.chameleonvision.vision.enums.TargetGroup;
+import com.chameleonvision.vision.enums.TargetIntersection;
+
+import java.util.Arrays;
+import java.util.List;
+
+public class CVPipeline2dSettings extends CVPipelineSettings {
+    public List<Number> hue = Arrays.asList(50, 180);
+    public List<Number> saturation = Arrays.asList(50, 255);
+    public List<Number> value = Arrays.asList(50, 255);
+    public boolean erode = false;
+    public boolean dilate = false;
+    public List<Number> area = Arrays.asList(0.0, 100.0);
+    public List<Number> ratio = Arrays.asList(0.0, 20.0);
+    public List<Number> extent = Arrays.asList(0, 100);
+    public Number speckle = 5;
+    public boolean isBinary = false;
+    public SortMode sortMode = SortMode.Largest;
+    public boolean multiple = false;
+    public TargetGroup targetGroup = TargetGroup.Single;
+    public TargetIntersection targetIntersection = TargetIntersection.Up;
+    public List<Number> point = Arrays.asList(0, 0);
+    public CalibrationMode calibrationMode = CalibrationMode.None;
+    public double dualTargetCalibrationM = 1;
+    public double dualTargetCalibrationB = 0;
+}

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

@@ -0,0 +1,35 @@
+package com.chameleonvision.vision.pipeline;
+
+import org.opencv.core.Mat;
+
+import java.util.List;
+
+import static com.chameleonvision.vision.pipeline.CVPipeline3d.*;
+
+public class CVPipeline3d extends CVPipeline<CVPipeline3dResult, CVPipeline3dSettings> {
+
+
+    protected CVPipeline3d(CVPipeline3dSettings settings) {
+        super(settings);
+    }
+
+    CVPipeline3d() {
+        super(new CVPipeline3dSettings());
+    }
+
+    @Override
+    public CVPipeline3dResult runPipeline(Mat inputMat) {
+        return null;
+    }
+
+
+    public static class CVPipeline3dResult extends CVPipelineResult<Target3d> {
+        public CVPipeline3dResult(List<Target3d> targets, Mat outputMat, long processTime) {
+            super(targets, outputMat, processTime);
+        }
+    }
+
+    public static class Target3d extends CVPipeline2d.Target2d {
+        // TODO: (2.1) Define 3d-specific target data
+    }
+}

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

@@ -0,0 +1,7 @@
+package com.chameleonvision.vision.pipeline;
+
+public class CVPipeline3dSettings extends CVPipeline2dSettings {
+    // TODO: (2.1) define 3d-specific pipeline settings
+    // add 3d-specific property to ensure serializing/deserializing works
+    public boolean placeholder = false;
+}

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

@@ -0,0 +1,24 @@
+package com.chameleonvision.vision.pipeline;
+
+import org.opencv.core.Mat;
+
+import java.util.List;
+
+public abstract class CVPipelineResult<T> {
+    public final List<T> targets;
+    public final boolean hasTarget;
+    public final Mat outputMat = new Mat();
+    public final long processTime;
+    public long imageTimestamp = 0;
+
+    public CVPipelineResult(List<T> targets, Mat outputMat, long processTime) {
+        this.targets = targets;
+        hasTarget = targets != null && !targets.isEmpty();
+        outputMat.copyTo(this.outputMat);
+        this.processTime = processTime;
+    }
+
+    public void setTimestamp(long timestamp) {
+        imageTimestamp = timestamp;
+    }
+}

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

@@ -0,0 +1,18 @@
+package com.chameleonvision.vision.pipeline;
+
+import com.chameleonvision.vision.enums.ImageFlipMode;
+import com.chameleonvision.vision.enums.ImageRotationMode;
+import com.chameleonvision.vision.enums.StreamDivisor;
+
+@SuppressWarnings("ALL")
+public class CVPipelineSettings {
+    public int index = 0;
+    public ImageFlipMode flipMode = ImageFlipMode.NONE;
+    public ImageRotationMode rotationMode = ImageRotationMode.DEG_0;
+    public String nickname = "New Pipeline";
+    public double exposure = 50.0;
+    public double brightness = 50.0;
+    public double gain = 0;
+    public int videoModeIndex = 0;
+    public StreamDivisor streamDivisor = StreamDivisor.NONE;
+}

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

@@ -0,0 +1,47 @@
+package com.chameleonvision.vision.pipeline;
+
+import com.chameleonvision.vision.camera.CameraCapture;
+import com.chameleonvision.vision.pipeline.pipes.Draw2dContoursPipe;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.RotatedRect;
+
+import java.util.List;
+
+import static com.chameleonvision.vision.pipeline.DriverVisionPipeline.DriverPipelineResult;
+
+public class DriverVisionPipeline extends CVPipeline<DriverPipelineResult, CVPipelineSettings> {
+
+    private Draw2dContoursPipe draw2dContoursPipe;
+    private Draw2dContoursPipe.Draw2dContoursSettings draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings();
+    private final List<RotatedRect> blankList = List.of();
+
+    public DriverVisionPipeline(CVPipelineSettings settings) {
+        super(settings);
+        settings.index = -1;
+    }
+
+    @Override
+    public void initPipeline(CameraCapture capture) {
+        super.initPipeline(capture);
+        draw2dContoursSettings.showCrosshair = true;
+        draw2dContoursPipe = new Draw2dContoursPipe(draw2dContoursSettings, cameraCapture.getProperties().getStaticProperties());
+    }
+
+    @Override
+    public DriverPipelineResult runPipeline(Mat inputMat) {
+
+        inputMat.copyTo(outputMat);
+
+        draw2dContoursPipe.setConfig(false, cameraCapture.getProperties().getStaticProperties());
+        draw2dContoursPipe.run(Pair.of(outputMat, blankList)).getLeft().copyTo(outputMat);
+
+        return new DriverPipelineResult(null, outputMat, 0);
+    }
+
+    public static class DriverPipelineResult extends CVPipelineResult<Void> {
+        public DriverPipelineResult(List<Void> targets, Mat outputMat, long processTime) {
+            super(targets, outputMat, processTime);
+        }
+    }
+}

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

@@ -0,0 +1,215 @@
+package com.chameleonvision.vision.pipeline;
+
+import com.chameleonvision.config.CameraConfig;
+import com.chameleonvision.config.ConfigManager;
+import com.chameleonvision.vision.VisionManager;
+import com.chameleonvision.vision.VisionProcess;
+import com.chameleonvision.web.SocketHandler;
+import edu.wpi.first.networktables.NetworkTableEntry;
+
+import java.util.Comparator;
+import java.util.HashMap;
+import java.util.LinkedList;
+import java.util.List;
+
+@SuppressWarnings("WeakerAccess")
+public class PipelineManager {
+
+    private static final int DRIVERMODE_INDEX = -1;
+
+    public final LinkedList<CVPipeline> pipelines = new LinkedList<>();
+
+    public final CVPipeline driverModePipeline = new DriverVisionPipeline(new CVPipelineSettings());
+
+    private final VisionProcess parentProcess;
+    private int lastPipelineIndex;
+    private int currentPipelineIndex;
+    public NetworkTableEntry ntIndexEntry;
+
+    public PipelineManager(VisionProcess visionProcess, List<CVPipelineSettings> loadedPipelineSettings) {
+        parentProcess = visionProcess;
+        if (loadedPipelineSettings == null || loadedPipelineSettings.size() == 0) {
+            pipelines.add(new CVPipeline2d("New Pipeline"));
+        } else {
+            for (CVPipelineSettings setting : loadedPipelineSettings) {
+                addInternalPipeline(setting);
+            }
+        }
+        driverModePipeline.initPipeline(visionProcess.getCamera());
+        setCurrentPipeline(0);
+    }
+
+    private void reassignIndexes() {
+        pipelines.sort(IndexComparator);
+        for (int i = 0; i < pipelines.size(); i++) {
+            pipelines.get(i).settings.index = i;
+        }
+    }
+
+    private CameraConfig getConfig(VisionProcess process) {
+        return VisionManager.getCameraConfig(process);
+    }
+
+    private CameraConfig getConfig() {
+        return getConfig(parentProcess);
+    }
+
+    private void savePipelineConfig(CVPipelineSettings setting) {
+        getConfig().pipelineConfig.save(setting);
+    }
+
+    private void deletePipelineConfig(CVPipelineSettings setting) {
+        getConfig().pipelineConfig.delete(setting);
+    }
+
+    private void renamePipelineConfig(CVPipelineSettings setting, String newName) {
+        getConfig().pipelineConfig.rename(setting, newName);
+    }
+
+    public void saveAllPipelines() {
+        pipelines.parallelStream().map(pipeline -> pipeline.settings).forEach(this::savePipelineConfig);
+    }
+
+    private void addInternalPipeline(CVPipelineSettings setting) {
+        if (setting instanceof CVPipeline3dSettings) {
+            pipelines.add(new CVPipeline3d((CVPipeline3dSettings) setting));
+        } else if (setting instanceof CVPipeline2dSettings) {
+            pipelines.add(new CVPipeline2d((CVPipeline2dSettings) setting));
+        } else {
+            System.out.println("Non 2D/3D pipelines not supported!");
+        }
+        reassignIndexes();
+    }
+
+    public void setDriverMode(boolean driverMode) {
+        if (driverMode) {
+            setCurrentPipeline(DRIVERMODE_INDEX);
+        } else {
+            setCurrentPipeline(lastPipelineIndex);
+        }
+    }
+
+    public boolean getDriverMode() {
+        return currentPipelineIndex == DRIVERMODE_INDEX;
+    }
+
+    public int getCurrentPipelineIndex() {
+        return currentPipelineIndex;
+    }
+
+    public CVPipeline getCurrentPipeline() {
+        if (currentPipelineIndex <= DRIVERMODE_INDEX) {
+            return driverModePipeline;
+        } else {
+            return pipelines.get(currentPipelineIndex);
+        }
+    }
+
+    public void setCurrentPipeline(int index) {
+        CVPipeline newPipeline;
+        if (index == DRIVERMODE_INDEX) {
+            newPipeline = driverModePipeline;
+
+            // if we're changing into driver mode, try to set the nt entry to frue
+            parentProcess.setDriverModeEntry(true);
+        } else {
+            newPipeline = pipelines.get(index);
+
+            // if we're switching out of driver mode, try to set the nt entry to false
+            parentProcess.setDriverModeEntry(false);
+        }
+        if (newPipeline != null) {
+            lastPipelineIndex = currentPipelineIndex;
+            currentPipelineIndex = index;
+            getCurrentPipeline().initPipeline(parentProcess.getCamera());
+
+            if(ConfigManager.settings.currentCamera.equals(parentProcess.getCamera().getProperties().name)) {
+                ConfigManager.settings.currentPipeline = currentPipelineIndex;
+
+                HashMap<String, Object> pipeChange = new HashMap<>();
+                pipeChange.put("currentPipeline", currentPipelineIndex);
+                SocketHandler.broadcastMessage(pipeChange);
+                try {
+                    SocketHandler.sendFullSettings();
+                } catch (Exception e) {
+                    // avoid NullPointerException when run before threads start
+                }
+            }
+            newPipeline.initPipeline(parentProcess.getCamera());
+            if(ntIndexEntry != null) {
+                ntIndexEntry.setDouble(index);
+            }
+        }
+    }
+
+    public void addPipeline(CVPipelineSettings setting) {
+        addInternalPipeline(setting);
+        savePipelineConfig(setting);
+    }
+
+    public void addPipeline(CVPipeline pipeline) {
+        pipelines.add(pipeline);
+        reassignIndexes();
+        savePipelineConfig(pipeline.settings);
+    }
+
+    public void addNewPipeline(boolean is3D) {
+        CVPipeline newPipeline;
+        if (!is3D) {
+            newPipeline = new CVPipeline2d();
+        } else {
+            newPipeline = new CVPipeline3d();
+        }
+        newPipeline.settings.index = pipelines.size();
+        addPipeline(newPipeline);
+    }
+
+    public CVPipeline getPipeline(int index) {
+        return pipelines.get(index);
+    }
+
+    public void duplicatePipeline(CVPipelineSettings pipeline) {
+        duplicatePipeline(pipeline, parentProcess);
+    }
+
+    public void duplicatePipeline(CVPipelineSettings pipeline, VisionProcess destinationProcess) {
+        pipeline.index = destinationProcess.pipelineManager.pipelines.size();
+        pipeline.nickname += "(Copy)";
+        destinationProcess.pipelineManager.addPipeline(pipeline);
+    }
+
+    public void renameCurrentPipeline(String newName) {
+        CVPipelineSettings settings = getCurrentPipeline().settings;
+        settings.nickname = newName;
+        renamePipelineConfig(settings, newName);
+    }
+
+    public void deleteCurrentPipeline() {
+        deletePipeline(currentPipelineIndex);
+    }
+
+    private void deletePipeline(int index) {
+        if (index == currentPipelineIndex) {
+            currentPipelineIndex -= 1;
+        }
+        deletePipelineConfig(getPipeline(index).settings);
+        pipelines.remove(index);
+        reassignIndexes();
+    }
+
+    public void saveDriverModeConfig() {
+        getConfig().saveDriverMode(driverModePipeline.settings);
+    }
+
+    private static final Comparator<CVPipeline> IndexComparator = (o1, o2) -> {
+        int o1Index = o1.settings.index;
+        int o2Index = o2.settings.index;
+
+        if (o1Index == o2Index) {
+            return 0;
+        } else if (o1Index < o2Index) {
+            return -1;
+        }
+        return 1;
+    };
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/BlurPipe.java

@@ -0,0 +1,44 @@
+package com.chameleonvision.vision.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.CvException;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+public class BlurPipe implements Pipe<Mat, Mat> {
+
+    private int blurSize;
+
+    private Mat processBuffer = new Mat();
+    private Mat outputMat = new Mat();
+
+    public BlurPipe(int blurSize) {
+        this.blurSize = blurSize;
+    }
+
+    public void setConfig(int blurSize) {
+        this.blurSize = blurSize;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Mat input) {
+        long processStartNanos = System.nanoTime();
+
+        if (blurSize > 0) {
+            input.copyTo(processBuffer);
+            try {
+                Imgproc.blur(processBuffer, processBuffer, new Size(blurSize, blurSize));
+                processBuffer.copyTo(outputMat);
+                processBuffer.release();
+            } catch (CvException e) {
+                System.err.println("(BlurPipe) Exception thrown by OpenCV: \n" + e.getMessage());
+            }
+        } else {
+            input.copyTo(outputMat);
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(outputMat, processTime);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Collect2dTargetsPipe.java

@@ -0,0 +1,88 @@
+package com.chameleonvision.vision.pipeline.pipes;
+
+import com.chameleonvision.vision.camera.CaptureStaticProperties;
+import com.chameleonvision.vision.pipeline.CVPipeline2d;
+import com.chameleonvision.vision.enums.CalibrationMode;
+import org.apache.commons.lang3.tuple.Pair;
+import org.apache.commons.math3.util.FastMath;
+import org.opencv.core.Mat;
+import org.opencv.core.RotatedRect;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class Collect2dTargetsPipe implements Pipe<Pair<List<RotatedRect>, CaptureStaticProperties>, List<CVPipeline2d.Target2d>> {
+
+    private CalibrationMode calibrationMode;
+    private CaptureStaticProperties camProps;
+    private List<Number> calibrationPoint;
+    private double calibrationM, calibrationB;
+
+    private List<CVPipeline2d.Target2d> targets = new ArrayList<>();
+
+    public Collect2dTargetsPipe(CalibrationMode calibrationMode, List<Number> calibrationPoint,
+                                double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
+        this.calibrationMode = calibrationMode;
+        this.camProps = camProps;
+        this.calibrationPoint = calibrationPoint;
+        this.calibrationM = calibrationM;
+        this.calibrationB = calibrationB;
+    }
+
+    public void setConfig(CalibrationMode calibrationMode, List<Number> calibrationPoint,
+                          double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
+        this.calibrationMode = calibrationMode;
+        this.camProps = camProps;
+        this.calibrationPoint = calibrationPoint;
+        this.calibrationM = calibrationM;
+        this.calibrationB = calibrationB;
+    }
+
+    @Override
+    public Pair<List<CVPipeline2d.Target2d>, Long> run(Pair<List<RotatedRect>, CaptureStaticProperties> inputPair) {
+        long processStartNanos = System.nanoTime();
+
+        targets.clear();
+        var input = inputPair.getLeft();
+        var imageArea = inputPair.getRight().imageArea;
+
+        if (input.size() > 0) {
+            for (RotatedRect r : input) {
+                CVPipeline2d.Target2d t = new CVPipeline2d.Target2d();
+                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() / imageArea;
+
+                targets.add(t);
+            }
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        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));
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Draw2dContoursPipe.java

@@ -0,0 +1,106 @@
+package com.chameleonvision.vision.pipeline.pipes;
+
+import com.chameleonvision.vision.camera.CaptureStaticProperties;
+import com.chameleonvision.util.Helpers;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Point;
+import org.opencv.core.*;
+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 CaptureStaticProperties camProps;
+
+    private Mat processBuffer = new Mat();
+    private Mat outputMat = new Mat();
+
+    private Point[] vertices = new Point[4];
+    private List<MatOfPoint> drawnContours = new ArrayList<>();
+    @SuppressWarnings("FieldCanBeLocal")
+    private Point xMax = new Point(), xMin = new Point(), yMax = new Point(), yMin = new Point();
+
+
+    public Draw2dContoursPipe(Draw2dContoursSettings settings, CaptureStaticProperties camProps) {
+        this.settings = settings;
+        this.camProps = camProps;
+    }
+
+    public void setConfig(boolean showMultiple,CaptureStaticProperties captureProps) {
+        settings.showMultiple = showMultiple;
+        camProps = captureProps;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Pair<Mat, List<RotatedRect>> input) {
+        long processStartNanos = System.nanoTime();
+
+        if (settings.showCrosshair || settings.showCentroid || settings.showMaximumBox || settings.showRotatedBox) {
+            input.getLeft().copyTo(processBuffer);
+
+            if (input.getRight().size() > 0) {
+                for (int i = 0; i < input.getRight().size(); i++) {
+                    if (i != 0 && !settings.showMultiple){
+                        break;
+                    }
+                    RotatedRect r = input.getRight().get(i);
+                    if (r == null) continue;
+
+                    drawnContours.forEach(Mat::release);
+                    drawnContours.clear();
+
+                    r.points(vertices);
+                    MatOfPoint contour = new MatOfPoint(vertices);
+                    drawnContours.add(contour);
+
+                    if (settings.showCentroid) {
+                        Imgproc.circle(processBuffer, r.center, 3, Helpers.colorToScalar(settings.centroidColor));
+                    }
+
+                    if (settings.showRotatedBox) {
+                        Imgproc.drawContours(processBuffer, drawnContours, 0, Helpers.colorToScalar(settings.rotatedBoxColor), settings.boxOutlineSize);
+                    }
+
+                    if (settings.showMaximumBox) {
+                        Rect box = Imgproc.boundingRect(contour);
+                        Imgproc.rectangle(processBuffer, new Point(box.x, box.y), new Point((box.x + box.width), (box.y + box.height)), Helpers.colorToScalar(settings.maximumBoxColor), settings.boxOutlineSize);
+                    }
+                }
+            }
+
+            if (settings.showCrosshair) {
+                xMax.set(new double[] {camProps.centerX + 10, camProps.centerY});
+                xMin.set(new double[] {camProps.centerX - 10, camProps.centerY});
+                yMax.set(new double[] {camProps.centerX, camProps.centerY + 10});
+                yMin.set(new double[] {camProps.centerX, camProps.centerY - 10});
+                Imgproc.line(processBuffer, xMax, xMin, Helpers.colorToScalar(settings.crosshairColor), 2);
+                Imgproc.line(processBuffer, yMax, yMin, Helpers.colorToScalar(settings.crosshairColor), 2);
+            }
+
+            processBuffer.copyTo(outputMat);
+            processBuffer.release();
+        } else {
+            input.getLeft().copyTo(outputMat);
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(outputMat, processTime);
+    }
+
+    public static class Draw2dContoursSettings {
+        public boolean showCentroid = false;
+        public boolean showCrosshair = false;
+        public boolean showMultiple = 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;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/ErodeDilatePipe.java

@@ -0,0 +1,53 @@
+package com.chameleonvision.vision.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 boolean erode;
+    private boolean dilate;
+    private Mat kernel;
+
+    private Mat processBuffer = new Mat();
+    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));
+    }
+
+    public void setConfig(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 || dilate) {
+            input.copyTo(processBuffer);
+
+            if (erode) {
+                Imgproc.erode(processBuffer, processBuffer, kernel);
+            }
+
+            if (dilate) {
+                Imgproc.dilate(processBuffer, processBuffer, kernel);
+            }
+
+            processBuffer.copyTo(outputMat);
+            processBuffer.release();
+        } else {
+            input.copyTo(outputMat);
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(outputMat, processTime);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/FilterContoursPipe.java

@@ -0,0 +1,75 @@
+package com.chameleonvision.vision.pipeline.pipes;
+
+import com.chameleonvision.vision.camera.CaptureStaticProperties;
+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 List<Number> area;
+    private List<Number> ratio;
+    private List<Number> extent;
+    private CaptureStaticProperties camProps;
+
+    private List<MatOfPoint> filteredContours = new ArrayList<>();
+
+    public FilterContoursPipe(List<Number> area, List<Number> ratio, List<Number> extent, CaptureStaticProperties camProps) {
+        this.area = area;
+        this.ratio = ratio;
+        this.extent = extent;
+        this.camProps = camProps;
+    }
+
+    public void setConfig(List<Number> area, List<Number> ratio, List<Number> extent, CaptureStaticProperties 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();
+
+        filteredContours.clear();
+
+        if (input.size() > 0) {
+            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 = System.nanoTime() - processStartNanos;
+        return Pair.of(filteredContours, processTime);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/FindContoursPipe.java

@@ -0,0 +1,28 @@
+package com.chameleonvision.vision.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();
+
+        foundContours.clear();
+
+        Imgproc.findContours(input, foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1);
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(foundContours, processTime);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java

@@ -0,0 +1,165 @@
+package com.chameleonvision.vision.pipeline.pipes;
+
+import com.chameleonvision.util.MathHandler;
+import com.chameleonvision.vision.enums.TargetGroup;
+import com.chameleonvision.vision.enums.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 TargetGroup group;
+    private 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;
+    }
+
+    public void setConfig(TargetGroup group, TargetIntersection intersection) {
+        this.group = group;
+        this.intersection = intersection;
+    }
+
+    @Override
+    public Pair<List<RotatedRect>, Long> run(List<MatOfPoint> input) {
+        long processStartNanos = System.nanoTime();
+
+        groupedContours.clear();
+
+        if (input.size() > (group.equals(TargetGroup.Single) ? 0 : 1)) {
+
+            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;
+                            }
+
+                            intersectMatA.release();
+                            intersectMatB.release();
+                            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) {
+                            finalContourList.clear();
+                        }
+                    }
+                    break;
+                }
+            }
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        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;
+        }
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/HsvPipe.java

@@ -0,0 +1,48 @@
+package com.chameleonvision.vision.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Core;
+import org.opencv.core.CvException;
+import org.opencv.core.Mat;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+
+public class HsvPipe implements Pipe<Mat, Mat> {
+
+    private Scalar hsvLower;
+    private Scalar hsvUpper;
+
+    private Mat processBuffer = new Mat();
+    private Mat outputMat = new Mat();
+
+    public HsvPipe(Scalar hsvLower, Scalar hsvUpper) {
+        this.hsvLower = hsvLower;
+        this.hsvUpper = hsvUpper;
+    }
+
+    public void setConfig(Scalar hsvLower, Scalar hsvUpper) {
+        this.hsvLower = hsvLower;
+        this.hsvUpper = hsvUpper;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Mat input) {
+        long processStartNanos = System.nanoTime();
+
+        input.copyTo(processBuffer);
+
+        try {
+            Imgproc.cvtColor(processBuffer, processBuffer, Imgproc.COLOR_BGR2HSV, 3);
+            Core.inRange(processBuffer, hsvLower, hsvUpper, processBuffer);
+        } catch (CvException e) {
+            System.err.println("(HsvPipe) Exception thrown by OpenCV: \n" + e.getMessage());
+        }
+
+        processBuffer.copyTo(outputMat);
+        processBuffer.release();
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(outputMat, processTime);
+    }
+}
+

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/OutputMatPipe.java

@@ -0,0 +1,49 @@
+package com.chameleonvision.vision.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.CvException;
+import org.opencv.core.Mat;
+import org.opencv.imgproc.Imgproc;
+
+public class OutputMatPipe implements Pipe<Pair<Mat, Mat>, Mat> {
+
+    private boolean showThresholded;
+
+    private Mat processBuffer = new Mat();
+    private Mat outputMat = new Mat();
+
+    public OutputMatPipe(boolean showThresholded) {
+        this.showThresholded = showThresholded;
+    }
+
+    public void setConfig(boolean showThresholded) {
+        this.showThresholded = showThresholded;
+    }
+
+    /**
+     *
+     * @param input Input object for pipe
+     *        Left is raw camera mat (8UC3), Right is HSV threshold mat (8UC1)
+     * @return Returns desired output Mat, and processing time in nanoseconds
+     */
+    @Override
+    public Pair<Mat, Long> run(Pair<Mat, Mat> input) {
+        long processStartNanos = System.nanoTime();
+
+        if (showThresholded) {
+            try {
+                input.getRight().copyTo(processBuffer);
+                Imgproc.cvtColor(processBuffer, processBuffer, Imgproc.COLOR_GRAY2BGR, 3);
+                processBuffer.copyTo(outputMat);
+                processBuffer.release();
+            } catch (CvException e) {
+                System.err.println("(OutputMat) Exception thrown by OpenCV: \n" + e.getMessage());
+            }
+        } else {
+            input.getLeft().copyTo(outputMat);
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(outputMat, processTime);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Pipe.java

@@ -0,0 +1,13 @@
+package com.chameleonvision.vision.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);
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/RotateFlipPipe.java

@@ -0,0 +1,54 @@
+package com.chameleonvision.vision.pipeline.pipes;
+
+import com.chameleonvision.vision.enums.ImageFlipMode;
+import com.chameleonvision.vision.enums.ImageRotationMode;
+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 ImageRotationMode rotation;
+    private ImageFlipMode flip;
+
+    private Mat processBuffer = new Mat();
+    private Mat outputMat = new Mat();
+
+    public RotateFlipPipe(ImageRotationMode rotation, ImageFlipMode flip) {
+        this.rotation = rotation;
+        this.flip = flip;
+    }
+
+    public void setConfig(ImageRotationMode rotation, ImageFlipMode flip) {
+        this.rotation = rotation;
+        this.flip = flip;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Mat input) {
+        long processStartNanos = System.nanoTime();
+
+        boolean shouldFlip = !flip.equals(ImageFlipMode.NONE);
+        boolean shouldRotate = !rotation.equals(ImageRotationMode.DEG_0);
+
+        if (shouldFlip || shouldRotate) {
+            input.copyTo(processBuffer);
+
+            if (shouldFlip) {
+                Core.flip(processBuffer, processBuffer, flip.value);
+            }
+
+            if (shouldRotate) {
+                Core.rotate(processBuffer, processBuffer, rotation.value);
+            }
+
+            processBuffer.copyTo(outputMat);
+            processBuffer.release();
+        } else {
+            input.copyTo(outputMat);
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(outputMat, processTime);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SortContoursPipe.java

@@ -0,0 +1,87 @@
+package com.chameleonvision.vision.pipeline.pipes;
+
+import com.chameleonvision.vision.camera.CaptureStaticProperties;
+import com.chameleonvision.vision.enums.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 SortMode sort;
+    private CaptureStaticProperties camProps;
+    private int maxTargets;
+
+    private List<RotatedRect> sortedContours = new ArrayList<>();
+
+    public SortContoursPipe(SortMode sort, CaptureStaticProperties camProps, int maxTargets) {
+        this.sort = sort;
+        this.camProps = camProps;
+        this.maxTargets = maxTargets;
+    }
+
+    public void setConfig(SortMode sort, CaptureStaticProperties camProps, int maxTargets) {
+        this.sort = sort;
+        this.camProps = camProps;
+        this.maxTargets = maxTargets;
+    }
+
+    @Override
+    public Pair<List<RotatedRect>, Long> run(List<RotatedRect> input) {
+        long processStartNanos = System.nanoTime();
+
+        sortedContours.clear();
+
+        if (input.size() > 0) {
+            sortedContours.addAll(input.subList(0, Math.min(input.size(), maxTargets - 1)));
+
+            switch (sort) {
+                case Largest:
+                    sortedContours.sort(SortByLargestComparator);
+                    break;
+                case Smallest:
+                    sortedContours.sort(SortBySmallestComparator);
+                    break;
+                case Highest:
+                    sortedContours.sort(SortByHighestComparator);
+                    break;
+                case Lowest:
+                    sortedContours.sort(SortByLowestComparator);
+                    break;
+                case Leftmost:
+                    sortedContours.sort(SortByLeftmostComparator);
+                    break;
+                case Rightmost:
+                    sortedContours.sort(SortByRightmostComparator);
+                    break;
+                case Centermost:
+                    sortedContours.sort(SortByCentermostComparator);
+                    break;
+                default:
+                    break;
+            }
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        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));
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SpeckleRejectPipe.java

@@ -0,0 +1,51 @@
+package com.chameleonvision.vision.pipeline.pipes;
+
+import org.apache.commons.lang3.tuple.Pair;
+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 double minPercentOfAvg;
+
+    private List<MatOfPoint> despeckledContours = new ArrayList<>();
+
+    public SpeckleRejectPipe(double minPercentOfAvg) {
+        this.minPercentOfAvg = minPercentOfAvg;
+    }
+
+    public void setConfig(double minPercentOfAvg) {
+        this.minPercentOfAvg = minPercentOfAvg;
+    }
+
+    @Override
+    public Pair<List<MatOfPoint>, Long> run(List<MatOfPoint> input) {
+        long processStartNanos = System.nanoTime();
+
+        despeckledContours.clear();
+
+        if (input.size() > 0) {
+            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 = System.nanoTime() - processStartNanos;
+        return Pair.of(despeckledContours, processTime);
+    }
+}