Rename to PhotonVision

photon-server/src/main/java/org/photonvision/common/calibration/CameraCalibrationCoefficients.java

@@ -0,0 +1,46 @@
+package org.photonvision.common.calibration;
+
+import org.photonvision.common.vision.opencv.Releasable;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnore;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.Size;
+
+public class CameraCalibrationCoefficients implements Releasable {
+    @JsonProperty("resolution")
+    public final Size resolution;
+
+    @JsonProperty("cameraIntrinsics")
+    public final JsonMat cameraIntrinsics;
+
+    @JsonProperty("cameraExtrinsics")
+    public final JsonMat cameraExtrinsics;
+
+    @JsonCreator
+    public CameraCalibrationCoefficients(
+            @JsonProperty("resolution") Size resolution,
+            @JsonProperty("cameraIntrinsics") JsonMat cameraIntrinsics,
+            @JsonProperty("cameraExtrinsics") JsonMat cameraExtrinsics) {
+        this.resolution = resolution;
+        this.cameraIntrinsics = cameraIntrinsics;
+        this.cameraExtrinsics = cameraExtrinsics;
+    }
+
+    @JsonIgnore
+    public Mat getCameraIntrinsicsMat() {
+        return cameraIntrinsics.getAsMat();
+    }
+
+    @JsonIgnore
+    public MatOfDouble getCameraExtrinsicsMat() {
+        return cameraExtrinsics.getAsMatOfDouble();
+    }
+
+    @Override
+    public void release() {
+        cameraIntrinsics.release();
+        cameraExtrinsics.release();
+    }
+}

photon-server/src/main/java/org/photonvision/common/calibration/JsonMat.java

@@ -0,0 +1,92 @@
+package org.photonvision.common.calibration;
+
+import org.photonvision.common.vision.opencv.Releasable;
+import com.fasterxml.jackson.annotation.JsonIgnore;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import java.util.Arrays;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+
+public class JsonMat implements Releasable {
+    public final int rows;
+    public final int cols;
+    public final int type;
+    public final double[] data;
+
+    @JsonIgnore private Mat wrappedMat;
+    private MatOfDouble wrappedMatOfDouble;
+
+    public JsonMat(int rows, int cols, double[] data) {
+        this(rows, cols, CvType.CV_64FC1, data);
+    }
+
+    public JsonMat(
+            @JsonProperty("rows") int rows,
+            @JsonProperty("cols") int cols,
+            @JsonProperty("type") int type,
+            @JsonProperty("data") double[] data) {
+        this.rows = rows;
+        this.cols = cols;
+        this.type = type;
+        this.data = data;
+    }
+
+    private static boolean isCameraMatrixMat(Mat mat) {
+        return mat.type() == CvType.CV_64FC1 && mat.cols() == 3 && mat.rows() == 3;
+    }
+
+    private static boolean isDistortionCoeffsMat(Mat mat) {
+        return mat.type() == CvType.CV_64FC1 && mat.cols() == 5 && mat.rows() == 1;
+    }
+
+    private static boolean isCalibrationMat(Mat mat) {
+        return isDistortionCoeffsMat(mat) || isCameraMatrixMat(mat);
+    }
+
+    @JsonIgnore
+    public static double[] getDataFromMat(Mat mat) {
+        if (!isCalibrationMat(mat)) return null;
+
+        double[] data = new double[(int) (mat.total() * mat.elemSize())];
+        mat.get(0, 0, data);
+
+        int dataLen = -1;
+
+        if (isCameraMatrixMat(mat)) dataLen = 9;
+        if (isDistortionCoeffsMat(mat)) dataLen = 5;
+
+        // truncate Mat data to correct number data points.
+        return Arrays.copyOfRange(data, 0, dataLen);
+    }
+
+    public static JsonMat fromMat(Mat mat) {
+        if (!isCalibrationMat(mat)) return null;
+        return new JsonMat(mat.rows(), mat.cols(), getDataFromMat(mat));
+    }
+
+    @JsonIgnore
+    public Mat getAsMat() {
+        if (this.type != CvType.CV_64FC1) return null;
+
+        if (wrappedMat == null) {
+            this.wrappedMat = new Mat(this.rows, this.cols, this.type);
+            this.wrappedMat.put(0, 0, this.data);
+        }
+        return this.wrappedMat;
+    }
+
+    @JsonIgnore
+    public MatOfDouble getAsMatOfDouble() {
+        if (this.wrappedMatOfDouble == null) {
+            this.wrappedMatOfDouble = new MatOfDouble();
+            getAsMat().convertTo(wrappedMatOfDouble, CvType.CV_64F);
+        }
+        return this.wrappedMatOfDouble;
+    }
+
+    @Override
+    public void release() {
+        getAsMat().release();
+    }
+}

photon-server/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java

@@ -0,0 +1,88 @@
+package org.photonvision.common.configuration;
+
+import org.photonvision.common.calibration.CameraCalibrationCoefficients;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.common.vision.camera.CameraType;
+import org.photonvision.common.vision.pipeline.CVPipelineSettings;
+import org.photonvision.common.vision.pipeline.DriverModePipelineSettings;
+import org.photonvision.common.vision.processes.PipelineManager;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnore;
+import com.fasterxml.jackson.annotation.JsonProperty;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class CameraConfiguration {
+    private static final Logger logger = new Logger(CameraConfiguration.class, LogGroup.Camera);
+
+    public String baseName = "";
+    public String uniqueName = "";
+    public String nickname = "";
+    public double FOV = 70;
+    public String path = "";
+    public CameraType cameraType = CameraType.UsbCamera;
+    public CameraCalibrationCoefficients calibration;
+    public List<Integer> CameraLEDs = new ArrayList<>();
+
+    public CameraConfiguration(String baseName, String path) {
+        this(baseName, baseName, baseName, path);
+    }
+
+    public CameraConfiguration(String baseName, String uniqueName, String nickname, String path) {
+        this.baseName = baseName;
+        this.uniqueName = uniqueName;
+        this.nickname = nickname;
+        this.path = path;
+    }
+
+    @JsonCreator
+    public CameraConfiguration(
+            @JsonProperty("baseName") String baseName,
+            @JsonProperty("uniqueName") String uniqueName,
+            @JsonProperty("nickname") String nickname,
+            @JsonProperty("FOV") double FOV,
+            @JsonProperty("path") String path,
+            @JsonProperty("cameraType") CameraType cameraType,
+            @JsonProperty("calibration") CameraCalibrationCoefficients calibration,
+            @JsonProperty("CameraLEDs") List<Integer> cameraLEDs) {
+        this.baseName = baseName;
+        this.uniqueName = uniqueName;
+        this.nickname = nickname;
+        this.FOV = FOV;
+        this.path = path;
+        this.cameraType = cameraType;
+        this.calibration = calibration;
+        this.CameraLEDs = cameraLEDs;
+    }
+
+    @JsonIgnore // this ignores the pipes as we serialize them to their own subfolder
+    public final List<CVPipelineSettings> pipelineSettings = new ArrayList<>();
+
+    @JsonIgnore
+    public DriverModePipelineSettings driveModeSettings = new DriverModePipelineSettings();
+
+    public void addPipelineSettings(List<CVPipelineSettings> settings) {
+        for (var setting : settings) {
+            addPipelineSetting(setting);
+        }
+    }
+
+    public void addPipelineSetting(CVPipelineSettings setting) {
+        if (pipelineSettings.stream()
+                .anyMatch(s -> s.pipelineNickname.equalsIgnoreCase(setting.pipelineNickname))) {
+            logger.error("Could not name two pipelines the same thing! Renaming");
+            setting.pipelineNickname += "_1"; // TODO verify this logic
+        }
+
+        if (pipelineSettings.stream().anyMatch(s -> s.pipelineIndex == setting.pipelineIndex)) {
+            var newIndex = pipelineSettings.size();
+            logger.error("Could not insert two pipelines at same index! Changing to " + newIndex);
+            setting.pipelineIndex = newIndex; // TODO verify this logic
+        }
+
+        pipelineSettings.add(setting);
+        pipelineSettings.sort(PipelineManager.PipelineSettingsIndexComparator);
+    }
+}

photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java

@@ -0,0 +1,247 @@
+package org.photonvision.common.configuration;
+
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.file.JacksonUtils;
+import org.photonvision.common.vision.pipeline.CVPipelineSettings;
+import org.photonvision.common.vision.pipeline.DriverModePipelineSettings;
+import com.fasterxml.jackson.core.JsonProcessingException;
+
+import java.io.File;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Objects;
+import java.util.stream.Collectors;
+
+public class ConfigManager {
+    private static final Logger logger = new Logger(ConfigManager.class, LogGroup.General);
+    private static ConfigManager INSTANCE;
+
+    private PhotonConfiguration config;
+    private final File rootFolder;
+    private final File hardwareConfigFile;
+    private final File networkConfigFile;
+    private final File camerasFolder;
+
+    public static ConfigManager getInstance() {
+        if (INSTANCE == null) {
+            INSTANCE = new ConfigManager(getRootFolder());
+        }
+        return INSTANCE;
+    }
+
+    public PhotonConfiguration getConfig() {
+        return config;
+    }
+
+    protected static Path getRootFolder() {
+        return Path.of("chameleon-vision");
+    }
+
+    private ConfigManager(Path rootFolder) {
+        this.rootFolder = new File(rootFolder.toUri());
+        this.hardwareConfigFile =
+                new File(Path.of(rootFolder.toString(), "hardwareConfig.json").toUri());
+        this.networkConfigFile =
+                new File(Path.of(rootFolder.toString(), "networkSettings.json").toUri());
+        this.camerasFolder = new File(Path.of(rootFolder.toString(), "cameras").toUri());
+        load();
+    }
+
+    public void load() {
+        logger.info("Loading settings...");
+        if (!rootFolder.exists()) {
+            if (rootFolder.mkdirs()) {
+                logger.debug("Root config folder did not exist. Created!");
+            } else {
+                logger.error("Failed to create root config folder!");
+            }
+        }
+
+        HardwareConfig hardwareConfig;
+        NetworkConfig networkConfig;
+
+        if (hardwareConfigFile.exists()) {
+            try {
+                hardwareConfig =
+                        JacksonUtils.deserialize(hardwareConfigFile.toPath(), HardwareConfig.class);
+                if (hardwareConfig == null) {
+                    logger.error("Could not deserialize hardware config! Loading defaults");
+                    hardwareConfig = new HardwareConfig();
+                }
+            } catch (IOException e) {
+                logger.error("Could not deserialize hardware config! Loading defaults");
+                hardwareConfig = new HardwareConfig();
+            }
+        } else {
+            logger.info("Hardware config does not exist! Loading defaults");
+            hardwareConfig = new HardwareConfig();
+        }
+
+        if (networkConfigFile.exists()) {
+            try {
+                networkConfig = JacksonUtils.deserialize(networkConfigFile.toPath(), NetworkConfig.class);
+                if (networkConfig == null) {
+                    logger.error("Could not deserialize network config! Loading defaults");
+                    networkConfig = new NetworkConfig();
+                }
+            } catch (IOException e) {
+                logger.error("Could not deserialize network config! Loading defaults");
+                networkConfig = new NetworkConfig();
+            }
+        } else {
+            logger.info("Network config file does not exist! Loading defaults");
+            networkConfig = new NetworkConfig();
+        }
+
+        if (!camerasFolder.exists()) {
+            if (camerasFolder.mkdirs()) {
+                logger.debug("Cameras config folder did not exist. Created!");
+            } else {
+                logger.error("Failed to create cameras config folder!");
+            }
+        }
+
+        HashMap<String, CameraConfiguration> cameraConfigurations = loadCameraConfigs();
+
+        this.config = new PhotonConfiguration(hardwareConfig, networkConfig, cameraConfigurations);
+    }
+
+    public void save() {
+        logger.info("Saving settings...");
+
+        try {
+            JacksonUtils.serializer(hardwareConfigFile.toPath(), config.getHardwareConfig());
+        } catch (IOException e) {
+            logger.error("Could not save hardware config!");
+            e.printStackTrace();
+        }
+        try {
+            JacksonUtils.serializer(networkConfigFile.toPath(), config.getNetworkConfig());
+        } catch (IOException e) {
+            logger.error("Could not save network config!");
+            e.printStackTrace();
+        }
+
+        // save all of our cameras
+        var cameraConfigMap = config.getCameraConfigurations();
+        for (var subdirName : cameraConfigMap.keySet()) {
+            var camConfig = cameraConfigMap.get(subdirName);
+            var subdir = Path.of(camerasFolder.toPath().toString(), subdirName);
+
+            if (!subdir.toFile().exists()) {
+                subdir.toFile().mkdirs();
+            }
+
+            try {
+                JacksonUtils.serializer(Path.of(subdir.toString(), "config.json"), camConfig);
+            } catch (IOException e) {
+                logger.error("Could not save config.json for " + subdir);
+            }
+
+            try {
+                JacksonUtils.serializer(
+                        Path.of(subdir.toString(), "drivermode.json"), camConfig.driveModeSettings);
+            } catch (IOException e) {
+                logger.error("Could not save drivermode.json for " + subdir);
+            }
+
+            for (var pipe : camConfig.pipelineSettings) {
+                var pipePath = Path.of(subdir.toString(), "pipelines", pipe.pipelineNickname + ".json");
+
+                if (!pipePath.getParent().toFile().exists()) {
+                    pipePath.getParent().toFile().mkdirs();
+                }
+
+                try {
+                    JacksonUtils.serializer(pipePath, pipe);
+                } catch (IOException e) {
+                    logger.error("Could not save " + pipe.pipelineNickname + ".json!");
+                }
+            }
+        }
+    }
+
+    private HashMap<String, CameraConfiguration> loadCameraConfigs() {
+        HashMap<String, CameraConfiguration> loadedConfigurations = new HashMap<>();
+        try {
+            var subdirectories =
+                    Files.list(camerasFolder.toPath())
+                            .filter(f -> f.toFile().isDirectory())
+                            .collect(Collectors.toList());
+
+            for (var subdir : subdirectories) {
+                var cameraConfigPath = Path.of(subdir.toString(), "config.json");
+                CameraConfiguration loadedConfig = null;
+                try {
+                    loadedConfig =
+                            JacksonUtils.deserialize(
+                                    cameraConfigPath.toAbsolutePath(), CameraConfiguration.class);
+                } catch (JsonProcessingException e) {
+                    e.printStackTrace();
+                }
+                if (loadedConfig == null) { // If the file could not be deserialized
+                    logger.warn("Could not load camera " + subdir + "'s config.json! Loading " + "default");
+                    continue; // TODO how do we later try to load this camera if it gets reconnected?
+                }
+
+                // At this point we have only loaded the base stuff
+                // We still need to deserialize pipelines, as well as
+                // driver mode settings
+                var driverModeFile = Path.of(subdir.toString(), "drivermode.json");
+                DriverModePipelineSettings driverMode;
+                try {
+                    driverMode =
+                            JacksonUtils.deserialize(
+                                    driverModeFile.toAbsolutePath(), DriverModePipelineSettings.class);
+                } catch (JsonProcessingException e) {
+                    logger.error("Could not deserialize drivermode.json! Loading defaults");
+                    logger.de_pest(Arrays.toString(e.getStackTrace()));
+                    driverMode = new DriverModePipelineSettings();
+                }
+                if (driverMode == null) {
+                    logger.warn(
+                            "Could not load camera " + subdir + "'s drivermode.json! Loading" + " default");
+                    driverMode = new DriverModePipelineSettings();
+                }
+
+                // Load pipelines by mapping the files within the pipelines subdir
+                // to their deserialized equivalents
+                List<CVPipelineSettings> settings =
+                        Files.list(Path.of(subdir.toString(), "pipelines"))
+                                .filter(p -> p.toFile().isFile())
+                                .map(
+                                        p -> {
+                                            var relativizedFilePath =
+                                                    getRootFolder().toAbsolutePath().relativize(p).toString();
+                                            try {
+                                                var ret = JacksonUtils.deserialize(p, CVPipelineSettings.class);
+                                                return ret;
+
+                                            } catch (JsonProcessingException e) {
+                                                logger.error("Exception while deserializing " + relativizedFilePath);
+                                                e.printStackTrace();
+                                            } catch (IOException e) {
+                                                logger.warn(
+                                                        "Could not load pipeline at " + relativizedFilePath + "! Skipping...");
+                                            }
+                                            return null;
+                                        })
+                                .filter(Objects::nonNull)
+                                .collect(Collectors.toList());
+
+                loadedConfig.driveModeSettings = driverMode;
+                loadedConfig.addPipelineSettings(settings);
+
+                loadedConfigurations.put(subdir.toFile().getName(), loadedConfig);
+            }
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+        return loadedConfigurations;
+    }
+}

photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java

@@ -0,0 +1,6 @@
+package org.photonvision.common.configuration;
+
+/** Defines a;lskdjfa;dsklf */
+public class HardwareConfig {
+    public int ledPin = 1; // just to stop jackson from yeeting
+}

photon-server/src/main/java/org/photonvision/common/configuration/NetworkConfig.java

@@ -0,0 +1,12 @@
+package org.photonvision.common.configuration;
+
+import org.photonvision.common.networking.NetworkMode;
+
+public class NetworkConfig {
+    public int teamNumber = 1577;
+    public NetworkMode connectionType = NetworkMode.DHCP;
+    public String ip = "";
+    public String gateway = "";
+    public String netmask = "";
+    public String hostname = "chameleon-vision";
+}

photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java

@@ -0,0 +1,44 @@
+package org.photonvision.common.configuration;
+
+import java.util.HashMap;
+
+// TODO rename this class
+public class PhotonConfiguration {
+    public HardwareConfig getHardwareConfig() {
+        return hardwareConfig;
+    }
+
+    public NetworkConfig getNetworkConfig() {
+        return networkConfig;
+    }
+
+    public HashMap<String, CameraConfiguration> getCameraConfigurations() {
+        return cameraConfigurations;
+    }
+
+    public void addCameraConfig(CameraConfiguration config) {
+        addCameraConfig(config.uniqueName, config);
+    }
+
+    public void addCameraConfig(String name, CameraConfiguration config) {
+        cameraConfigurations.put(name, config);
+    }
+
+    private HardwareConfig hardwareConfig;
+    private NetworkConfig networkConfig;
+
+    private HashMap<String, CameraConfiguration> cameraConfigurations;
+
+    public PhotonConfiguration(HardwareConfig hardwareConfig, NetworkConfig networkConfig) {
+        this(hardwareConfig, networkConfig, new HashMap<>());
+    }
+
+    public PhotonConfiguration(
+            HardwareConfig hardwareConfig,
+            NetworkConfig networkConfig,
+            HashMap<String, CameraConfiguration> cameraConfigurations) {
+        this.hardwareConfig = hardwareConfig;
+        this.networkConfig = networkConfig;
+        this.cameraConfigurations = cameraConfigurations;
+    }
+}

photon-server/src/main/java/org/photonvision/common/configuration/StreamDivisor.java

@@ -0,0 +1,14 @@
+package org.photonvision.common.configuration;
+
+public enum StreamDivisor {
+    NONE(1),
+    HALF(2),
+    QUARTER(4),
+    SIXTH(6);
+
+    public final Integer value;
+
+    StreamDivisor(int value) {
+        this.value = value;
+    }
+}

photon-server/src/main/java/org/photonvision/common/datatransfer/DataConsumer.java

@@ -0,0 +1,7 @@
+package org.photonvision.common.datatransfer;
+
+import org.photonvision.common.vision.processes.Data;
+
+import java.util.function.Consumer;
+
+public interface DataConsumer extends Consumer<Data> {}

photon-server/src/main/java/org/photonvision/common/datatransfer/DataProvider.java

@@ -0,0 +1,3 @@
+package org.photonvision.common.datatransfer;
+
+public interface DataProvider {}

photon-server/src/main/java/org/photonvision/common/datatransfer/networktables/NetworkTablesManager.java

@@ -0,0 +1,81 @@
+package org.photonvision.common.datatransfer.networktables;
+
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.common.scripting.ScriptEventType;
+import org.photonvision.common.scripting.ScriptManager;
+import edu.wpi.first.networktables.LogMessage;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+import java.util.function.Consumer;
+
+public class NetworkTablesManager {
+
+    private NetworkTablesManager() {}
+
+    private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General);
+
+    private static final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
+
+    public static final String kRootTableName = "/chameleon-vision";
+    public static final NetworkTable kRootTable =
+            NetworkTableInstance.getDefault().getTable(kRootTableName);
+
+    public static boolean isServer = false;
+
+    private static int getTeamNumber() {
+        // TODO: FIX
+        return 0;
+        //        return ConfigManager.settings.teamNumber;
+    }
+
+    private static class NTLogger implements Consumer<LogMessage> {
+
+        private boolean hasReportedConnectionFailure = false;
+
+        @Override
+        public void accept(LogMessage logMessage) {
+            if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) {
+                logger.error("NT Connection has failed! Will retry in background.");
+                hasReportedConnectionFailure = true;
+            } else if (logMessage.message.contains("connected")) {
+                logger.info("NT Connected!");
+                hasReportedConnectionFailure = false;
+                ScriptManager.queueEvent(ScriptEventType.kNTConnected);
+            }
+        }
+    }
+
+    static {
+        NetworkTableInstance.getDefault().addLogger(new NTLogger(), 0, 255); // to hide error messages
+    }
+
+    public static void setClientMode(String host) {
+        isServer = false;
+        logger.info("Starting NT Client");
+        ntInstance.stopServer();
+        if (host != null) {
+            ntInstance.startClient(host);
+        } else {
+            ntInstance.startClientTeam(getTeamNumber());
+            if (ntInstance.isConnected()) {
+                logger.info("[NetworkTablesManager] Connected to the robot!");
+            } else {
+                logger.info(
+                        "[NetworkTablesManager] Could NOT to the robot! Will retry in the background...");
+            }
+        }
+    }
+
+    public static void setTeamClientMode() {
+        setClientMode(null);
+    }
+
+    public static void setServerMode() {
+        isServer = true;
+        logger.info("Starting NT Server");
+        ntInstance.stopClient();
+        ntInstance.startServer();
+    }
+}

photon-server/src/main/java/org/photonvision/common/logging/Level.java

@@ -0,0 +1,19 @@
+package org.photonvision.common.logging;
+
+public enum Level {
+    OFF(0, Logger.ANSI_BLACK),
+    ERROR(1, Logger.ANSI_RED),
+    WARN(2, Logger.ANSI_YELLOW),
+    INFO(3, Logger.ANSI_GREEN),
+    DEBUG(4, Logger.ANSI_WHITE),
+    TRACE(5, Logger.ANSI_CYAN),
+    DE_PEST(6, Logger.ANSI_WHITE);
+
+    public final String colorCode;
+    public final int code;
+
+    Level(int code, String colorCode) {
+        this.code = code;
+        this.colorCode = colorCode;
+    }
+}

photon-server/src/main/java/org/photonvision/common/logging/LogGroup.java

@@ -0,0 +1,8 @@
+package org.photonvision.common.logging;
+
+public enum LogGroup {
+    Camera,
+    Server,
+    VisionProcess,
+    General
+}

photon-server/src/main/java/org/photonvision/common/logging/Logger.java

@@ -0,0 +1,157 @@
+package org.photonvision.common.logging;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+import java.nio.channels.AsynchronousFileChannel;
+import java.nio.file.Path;
+import java.nio.file.StandardOpenOption;
+import java.text.SimpleDateFormat;
+import java.util.ArrayList;
+import java.util.Date;
+import java.util.HashMap;
+import java.util.List;
+
+public class Logger {
+
+    private final String className;
+
+    public static final String ANSI_RESET = "\u001B[0m";
+    public static final String ANSI_BLACK = "\u001B[30m";
+    public static final String ANSI_RED = "\u001B[31m";
+    public static final String ANSI_GREEN = "\u001B[32m";
+    public static final String ANSI_YELLOW = "\u001B[33m";
+    public static final String ANSI_BLUE = "\u001B[34m";
+    public static final String ANSI_PURPLE = "\u001B[35m";
+    public static final String ANSI_CYAN = "\u001B[36m";
+    public static final String ANSI_WHITE = "\u001B[37m";
+
+    private static SimpleDateFormat simpleDateFormat = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss");
+    private final LogGroup group;
+
+    public Logger(Class<?> clazz, LogGroup group) {
+        this.className = clazz.getSimpleName();
+        this.group = group;
+    }
+
+    public static String getDate() {
+        return simpleDateFormat.format(new Date());
+    }
+
+    public static String format(
+            String logMessage, Level level, LogGroup group, String clazz, boolean color) {
+        var date = getDate();
+        var builder = new StringBuilder();
+        if (color) builder.append(level.colorCode);
+        builder
+                .append("[")
+                .append(date)
+                .append("] [")
+                .append(group)
+                .append(" - ")
+                .append(clazz)
+                .append("] [")
+                .append(level.name())
+                .append("] ")
+                .append(logMessage);
+        if (color) builder.append(ANSI_RESET);
+        return builder.toString();
+    }
+
+    private static HashMap<LogGroup, Level> levelMap = new HashMap<>();
+    private static List<Appender> currentAppenders = new ArrayList<>();
+
+    static {
+        levelMap.put(LogGroup.Camera, Level.INFO);
+        levelMap.put(LogGroup.General, Level.INFO);
+        levelMap.put(LogGroup.Server, Level.INFO);
+        levelMap.put(LogGroup.VisionProcess, Level.INFO);
+    }
+
+    static {
+        currentAppenders.add(new ConsoleAppender());
+    }
+
+    public static void addFileAppender(Path logFilePath) {
+        var file = logFilePath.toFile();
+        if (!file.exists()) {
+            try {
+                file.getParentFile().mkdirs();
+                file.createNewFile();
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+        }
+        currentAppenders.add(new AsyncFileAppender(logFilePath));
+    }
+
+    public static void setLevel(LogGroup group, Level newLevel) {
+        levelMap.put(group, newLevel);
+    }
+
+    private static void log(String message, Level level, LogGroup group, String clazz) {
+        for (var a : currentAppenders) {
+            var shouldColor = a instanceof ConsoleAppender;
+            var formattedMessage = format(message, level, group, clazz, shouldColor);
+            a.log(formattedMessage);
+        }
+    }
+
+    private static boolean shouldLog(Level logLevel, LogGroup group) {
+        return logLevel.code <= levelMap.get(group).code;
+    }
+
+    public void error(String message) {
+        if (shouldLog(Level.ERROR, group)) log(message, Level.ERROR, group, className);
+    }
+
+    public void warn(String message) {
+        if (shouldLog(Level.WARN, group)) log(message, Level.WARN, group, className);
+    }
+
+    public void info(String message) {
+        if (shouldLog(Level.INFO, group)) log(message, Level.INFO, group, className);
+    }
+
+    public void debug(String message) {
+        if (shouldLog(Level.DEBUG, group)) log(message, Level.DEBUG, group, className);
+    }
+
+    public void trace(String message) {
+        if (shouldLog(Level.TRACE, group)) log(message, Level.TRACE, group, className);
+    }
+
+    public void de_pest(String message) {
+        if (shouldLog(Level.DE_PEST, group)) log(message, Level.DE_PEST, group, className);
+    }
+
+    private abstract static class Appender {
+        abstract void log(String message);
+    }
+
+    private static class ConsoleAppender extends Appender {
+        @Override
+        void log(String message) {
+            System.out.println(message);
+        }
+    }
+
+    private static class AsyncFileAppender extends Appender {
+        private Path filePath;
+
+        public AsyncFileAppender(Path logFilePath) {
+            this.filePath = logFilePath;
+        }
+
+        @Override
+        void log(String message) {
+            try (AsynchronousFileChannel asyncFile =
+                    AsynchronousFileChannel.open(
+                            filePath, StandardOpenOption.WRITE, StandardOpenOption.CREATE)) {
+
+                asyncFile.write(ByteBuffer.wrap(message.getBytes()), 0);
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/networking/LinuxNetworking.java

@@ -0,0 +1,105 @@
+package org.photonvision.common.networking;
+
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import java.io.File;
+import java.io.IOException;
+import java.net.InetAddress;
+import java.net.SocketException;
+import java.nio.charset.StandardCharsets;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import org.apache.commons.io.FileUtils;
+
+public class LinuxNetworking extends SysNetworking {
+    private static final String PATH = "/etc/dhcpcd.conf";
+
+    private Logger logger = new Logger(LinuxNetworking.class, LogGroup.General);
+
+    @Override
+    public boolean setDHCP() {
+        File dhcpConf = new File(PATH);
+        logger.debug("Removing static IP from " + PATH);
+        if (dhcpConf.exists()) {
+            try {
+                List<String> lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8);
+                for (int i = 0; i < lines.size(); i++) {
+                    String line = lines.get(i);
+                    if (line.startsWith("interface " + networkInterface.name)) {
+                        lines.remove(i);
+                        for (int j = i; j < lines.size(); j++) {
+                            String subInterface = lines.get(j);
+                            if (subInterface.contains("static ip_address")
+                                    || subInterface.contains("static routers")) {
+                                lines.remove(j);
+                                j--;
+                            }
+                            if (subInterface.contains("interface")) {
+                                break;
+                            }
+                        }
+                        FileUtils.writeLines(dhcpConf, lines);
+                        return true;
+                    }
+                }
+            } catch (IOException e) {
+                e.printStackTrace();
+                return false;
+            }
+
+        } else {
+            logger.error("dhcpcd5 is not installed, unable to set IP.");
+            return false;
+        }
+        return true;
+    }
+
+    @Override
+    public boolean setHostname(String newHostname) {
+        try {
+            var setHostnameRetCode = shell.execute("hostnamectl", "set-hostname", newHostname);
+            return setHostnameRetCode == 0;
+        } catch (Exception e) {
+            e.printStackTrace();
+            return false;
+        }
+    }
+
+    @Override
+    public boolean setStatic(String ipAddress, String netmask, String gateway) {
+        setDHCP(); // clean up old static interface
+        File dhcpConf = new File(PATH);
+        try {
+            List<String> lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8);
+            lines.add("interface " + networkInterface.name);
+            InetAddress iNetMask = InetAddress.getByName(netmask);
+            int prefix = convertNetmaskToCIDR(iNetMask);
+            lines.add("static ip_address=" + ipAddress + "/" + prefix);
+            lines.add("static routers=" + gateway);
+            FileUtils.writeLines(dhcpConf, lines);
+            return true;
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+        return false;
+    }
+
+    @Override
+    public List<java.net.NetworkInterface> getNetworkInterfaces() throws SocketException {
+        List<java.net.NetworkInterface> netInterfaces;
+        try {
+            netInterfaces = Collections.list(java.net.NetworkInterface.getNetworkInterfaces());
+        } catch (SocketException e) {
+            return null;
+        }
+        List<java.net.NetworkInterface> goodInterfaces = new ArrayList<>();
+
+        for (var netInterface : netInterfaces) {
+            if (netInterface.getDisplayName().contains("lo")) continue;
+            if (!netInterface.isUp()) continue;
+            goodInterfaces.add(netInterface);
+        }
+        return goodInterfaces;
+    }
+}

photon-server/src/main/java/org/photonvision/common/networking/NetworkInterface.java

@@ -0,0 +1,59 @@
+package org.photonvision.common.networking;
+
+import java.net.InterfaceAddress;
+
+@SuppressWarnings("WeakerAccess")
+public class NetworkInterface {
+    public final String name;
+    public final String displayName;
+    public final String IPAddress;
+    public final String Netmask;
+    public final String Gateway;
+    public final String Broadcast;
+
+    public NetworkInterface(java.net.NetworkInterface inetface, InterfaceAddress ifaceAddress) {
+        name = inetface.getName();
+        displayName = inetface.getDisplayName();
+
+        var inetAddress = ifaceAddress.getAddress();
+        IPAddress = inetAddress.getHostAddress();
+        Netmask = getIPv4LocalNetMask(ifaceAddress);
+
+        // TODO: (low) hack to "get" gateway, this is gross and bad, pls fix
+        var splitIPAddr = IPAddress.split("\\.");
+        splitIPAddr[3] = "1";
+        Gateway = String.join(".", splitIPAddr);
+        splitIPAddr[3] = "255";
+        Broadcast = String.join(".", splitIPAddr);
+    }
+
+    private static String getIPv4LocalNetMask(InterfaceAddress interfaceAddress) {
+        var netPrefix = interfaceAddress.getNetworkPrefixLength();
+        try {
+            // Since this is for IPv4, it's 32 bits, so set the sign value of
+            // the int to "negative"...
+            int shiftby = (1 << 31);
+            // For the number of bits of the prefix -1 (we already set the sign bit)
+            for (int i = netPrefix - 1; i > 0; i--) {
+                // Shift the sign right... Java makes the sign bit sticky on a shift...
+                // So no need to "set it back up"...
+                shiftby = (shiftby >> 1);
+            }
+            // Transform the resulting value in xxx.xxx.xxx.xxx format, like if
+            /// it was a standard address...
+            // Return the address thus created...
+            return ((shiftby >> 24) & 255)
+                    + "."
+                    + ((shiftby >> 16) & 255)
+                    + "."
+                    + ((shiftby >> 8) & 255)
+                    + "."
+                    + (shiftby & 255);
+            //            return InetAddress.getByName(maskString);
+        } catch (Exception e) {
+            e.printStackTrace();
+        }
+        // Something went wrong here...
+        return null;
+    }
+}

photon-server/src/main/java/org/photonvision/common/networking/NetworkManager.java

@@ -0,0 +1,22 @@
+package org.photonvision.common.networking;
+
+public class NetworkManager {
+    private NetworkManager() {}
+
+    private static class SingletonHolder {
+        private static final NetworkManager INSTANCE = new NetworkManager();
+    }
+
+    public static NetworkManager getInstance() {
+        return SingletonHolder.INSTANCE;
+    }
+
+    private boolean isManaged = false;
+
+    public void initialize(boolean shouldManage) {
+        isManaged = shouldManage;
+        if (!isManaged) {
+            return;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/networking/NetworkMode.java

@@ -0,0 +1,6 @@
+package org.photonvision.common.networking;
+
+public enum NetworkMode {
+    DHCP,
+    STATIC
+}

photon-server/src/main/java/org/photonvision/common/networking/SysNetworking.java

@@ -0,0 +1,68 @@
+package org.photonvision.common.networking;
+
+import org.photonvision.common.util.ShellExec;
+
+import java.io.IOException;
+import java.net.InetAddress;
+import java.net.SocketException;
+import java.util.List;
+
+public abstract class SysNetworking {
+    NetworkInterface networkInterface;
+    ShellExec shell = new ShellExec(true, true);
+
+    private String hostname = getHostname();
+
+    public String getHostname() {
+        if (hostname == null) {
+            try {
+                var retCode = shell.execute("hostname", null, true);
+                if (retCode == 0) {
+                    while (!shell.isOutputCompleted()) {}
+                    return shell.getOutput();
+                } else {
+                    return null;
+                }
+            } catch (IOException e) {
+                return null;
+            }
+        } else return hostname;
+    }
+
+    // code belongs to
+    // https://stackoverflow.com/questions/19531411/calculate-cidr-from-a-given-netmask-java
+    public static int convertNetmaskToCIDR(InetAddress netmask) {
+
+        byte[] netmaskBytes = netmask.getAddress();
+        int cidr = 0;
+        boolean zero = false;
+        for (byte b : netmaskBytes) {
+            int mask = 0x80;
+
+            for (int i = 0; i < 8; i++) {
+                int result = b & mask;
+                if (result == 0) {
+                    zero = true;
+                } else if (zero) {
+                    throw new IllegalArgumentException("Invalid netmask.");
+                } else {
+                    cidr++;
+                }
+                mask >>>= 1;
+            }
+        }
+        return cidr;
+    }
+
+    public void setNetworkInterface(NetworkInterface networkInterface) {
+        this.networkInterface = networkInterface;
+    }
+
+    public abstract boolean setDHCP();
+
+    public abstract boolean setHostname(String hostname);
+
+    public abstract boolean setStatic(String ipAddress, String netmask, String gateway);
+
+    public abstract List<java.net.NetworkInterface> getNetworkInterfaces() throws SocketException;
+}

photon-server/src/main/java/org/photonvision/common/scripting/ScriptCommandType.java

@@ -0,0 +1,14 @@
+package org.photonvision.common.scripting;
+
+public enum ScriptCommandType {
+    kDefault(""),
+    kBashScript("bash"),
+    kPythonScript("python"),
+    kPython3Script("python3");
+
+    public final String value;
+
+    ScriptCommandType(String value) {
+        this.value = value;
+    }
+}

photon-server/src/main/java/org/photonvision/common/scripting/ScriptConfig.java

@@ -0,0 +1,22 @@
+package org.photonvision.common.scripting;
+
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonProperty;
+
+public class ScriptConfig {
+    public final ScriptEventType eventType;
+    public final String command;
+
+    public ScriptConfig(ScriptEventType eventType) {
+        this.eventType = eventType;
+        this.command = "";
+    }
+
+    @JsonCreator
+    public ScriptConfig(
+            @JsonProperty("eventType") ScriptEventType eventType,
+            @JsonProperty("command") String command) {
+        this.eventType = eventType;
+        this.command = command;
+    }
+}

photon-server/src/main/java/org/photonvision/common/scripting/ScriptEvent.java

@@ -0,0 +1,38 @@
+package org.photonvision.common.scripting;
+
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.ShellExec;
+
+import java.io.IOException;
+
+public class ScriptEvent {
+    private static final ShellExec executor = new ShellExec(true, true);
+
+    public final ScriptConfig config;
+    private final Logger logger = new Logger(ScriptEvent.class, LogGroup.General);
+
+    public ScriptEvent(ScriptConfig config) {
+        this.config = config;
+    }
+
+    public int run() throws IOException {
+        int retVal = executor.executeBashCommand(config.command);
+
+        String output = executor.getOutput();
+        String error = executor.getError();
+
+        if (!error.isEmpty()) {
+            System.err.printf("Error when running \"%s\" script: %s\n", config.eventType.name(), error);
+        } else if (!output.isEmpty()) {
+            logger.info(
+                    String.format("Output from \"%s\" script: %s\n", config.eventType.name(), output));
+        }
+        logger.info(
+                String.format(
+                        "Script for %s ran with command line: \"%s\", exit code: %d, output: %s, "
+                                + "error: %s\n",
+                        config.eventType.name(), config.command, retVal, output, error));
+        return retVal;
+    }
+}

photon-server/src/main/java/org/photonvision/common/scripting/ScriptEventType.java

@@ -0,0 +1,21 @@
+package org.photonvision.common.scripting;
+
+public enum ScriptEventType {
+    kProgramInit("Program Init"),
+    kProgramExit("Program Exit"),
+    kNTConnected("NT Connected"),
+    kLEDOn("LED On"),
+    kLEDOff("LED Off"),
+    kEnterDriverMode("Enter Driver Mode"),
+    kExitDriverMode("Exit Driver Mode"),
+    kFoundTarget("Found Target"),
+    kFoundMultipleTarget("Found Multiple Target"),
+    kLostTarget("Lost Target"),
+    kPipelineLag("Pipeline Lag");
+
+    public final String value;
+
+    ScriptEventType(String value) {
+        this.value = value;
+    }
+}

photon-server/src/main/java/org/photonvision/common/scripting/ScriptManager.java

@@ -0,0 +1,135 @@
+package org.photonvision.common.scripting;
+
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.LoopingRunnable;
+import org.photonvision.common.util.Platform;
+import org.photonvision.common.util.file.JacksonUtils;
+
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.concurrent.LinkedBlockingDeque;
+
+public class ScriptManager {
+
+    private static final Logger logger = new Logger(ScriptManager.class, LogGroup.General);
+
+    private ScriptManager() {}
+
+    private static final List<ScriptEvent> events = new ArrayList<>();
+    private static final LinkedBlockingDeque<ScriptEventType> queuedEvents =
+            new LinkedBlockingDeque<>(25);
+
+    public static void initialize() {
+        ScriptConfigManager.initialize();
+        if (ScriptConfigManager.fileExists()) {
+            for (ScriptConfig scriptConfig : ScriptConfigManager.loadConfig()) {
+                ScriptEvent scriptEvent = new ScriptEvent(scriptConfig);
+                events.add(scriptEvent);
+            }
+
+            new Thread(new ScriptRunner(10L)).start();
+        } else {
+            System.err.println("Something went wrong initializing scripts! Events will not run.");
+        }
+    }
+
+    private static class ScriptRunner extends LoopingRunnable {
+
+        ScriptRunner(Long loopTimeMs) {
+            super(loopTimeMs);
+        }
+
+        @Override
+        protected void process() {
+            try {
+
+                handleEvent(queuedEvents.takeFirst());
+            } catch (InterruptedException e) {
+                e.printStackTrace();
+            }
+        }
+
+        private void handleEvent(ScriptEventType eventType) {
+            var toRun =
+                    events
+                            .parallelStream()
+                            .filter(e -> e.config.eventType == eventType)
+                            .findFirst()
+                            .orElse(null);
+            if (toRun != null) {
+                try {
+                    toRun.run();
+                } catch (IOException e) {
+                    System.err.printf(
+                            "Failed to run script for event: %s, exception below.\n%s\n",
+                            eventType.name(), e.getMessage());
+                }
+            }
+        }
+    }
+
+    protected static class ScriptConfigManager {
+
+        //        protected static final Path scriptConfigPath =
+        // Paths.get(ConfigManager.SettingsPath.toString(), "scripts.json");
+        static final Path scriptConfigPath = Paths.get(""); // TODO: Waiting on config
+
+        private ScriptConfigManager() {}
+
+        static boolean fileExists() {
+            return Files.exists(scriptConfigPath);
+        }
+
+        public static void initialize() {
+            if (!fileExists()) {
+                List<ScriptConfig> eventsConfig = new ArrayList<>();
+                for (var eventType : ScriptEventType.values()) {
+                    eventsConfig.add(new ScriptConfig(eventType));
+                }
+
+                try {
+                    JacksonUtils.serializer(
+                            scriptConfigPath, eventsConfig.toArray(new ScriptConfig[0]), true);
+                } catch (IOException e) {
+                    e.printStackTrace();
+                }
+            }
+        }
+
+        static List<ScriptConfig> loadConfig() {
+            try {
+                var raw = JacksonUtils.deserialize(scriptConfigPath, ScriptConfig[].class);
+                if (raw != null) {
+                    return List.of(raw);
+                }
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+            return new ArrayList<>();
+        }
+
+        protected static void deleteConfig() {
+            try {
+                Files.delete(scriptConfigPath);
+            } catch (IOException e) {
+                //
+            }
+        }
+    }
+
+    public static void queueEvent(ScriptEventType eventType) {
+        if (!Platform.CurrentPlatform.isWindows()) {
+            try {
+                queuedEvents.putLast(eventType);
+                logger.info("Queued event: " + eventType.name());
+            } catch (InterruptedException e) {
+                System.err.println("Failed to add event to queue: " + eventType.name());
+            }
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/ColorHelper.java

@@ -0,0 +1,10 @@
+package org.photonvision.common.util;
+
+import java.awt.*;
+import org.opencv.core.Scalar;
+
+public class ColorHelper {
+    public static Scalar colorToScalar(Color color) {
+        return new Scalar(color.getBlue(), color.getGreen(), color.getRed());
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/LoopingRunnable.java

@@ -0,0 +1,36 @@
+package org.photonvision.common.util;
+
+/** A thread that tries to run at a specified loop time */
+public abstract class LoopingRunnable implements Runnable {
+    protected volatile Long loopTimeMs;
+
+    protected abstract void process();
+
+    public LoopingRunnable(Long loopTimeMs) {
+        this.loopTimeMs = loopTimeMs;
+    }
+
+    @Override
+    public void run() {
+        while (!Thread.interrupted()) {
+            var now = System.currentTimeMillis();
+
+            // Do the thing
+            process();
+
+            // sleep for the remaining time
+            var timeElapsed = System.currentTimeMillis() - now;
+            var delta = loopTimeMs - timeElapsed;
+            try {
+                if (delta > 0.0) {
+
+                    Thread.sleep(delta, 0);
+
+                } else {
+                    Thread.sleep(1);
+                }
+            } catch (Exception ignored) {
+            }
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/MemoryManager.java

@@ -0,0 +1,68 @@
+package org.photonvision.common.util;
+
+public class MemoryManager {
+
+    private static final long MEGABYTE_FACTOR = 1024L * 1024L;
+
+    private int collectionThreshold;
+    private long collectionPeriodMillis = -1;
+
+    private double lastUsedMb = 0;
+    private long lastCollectionMillis = 0;
+
+    public MemoryManager(int collectionThreshold) {
+        this.collectionThreshold = collectionThreshold;
+    }
+
+    public MemoryManager(int collectionThreshold, long collectionPeriodMillis) {
+        this.collectionThreshold = collectionThreshold;
+        this.collectionPeriodMillis = collectionPeriodMillis;
+    }
+
+    public void setCollectionThreshold(int collectionThreshold) {
+        this.collectionThreshold = collectionThreshold;
+    }
+
+    public void setCollectionPeriodMillis(long collectionPeriodMillis) {
+        this.collectionPeriodMillis = collectionPeriodMillis;
+    }
+
+    private static long getUsedMemory() {
+        return Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory();
+    }
+
+    private static double getUsedMemoryMB() {
+        return ((double) getUsedMemory() / MEGABYTE_FACTOR);
+    }
+
+    private void collect() {
+        System.gc();
+        System.runFinalization();
+    }
+
+    public void run() {
+        run(false);
+    }
+
+    public void run(boolean print) {
+        var usedMem = getUsedMemoryMB();
+
+        if (usedMem != lastUsedMb) {
+            lastUsedMb = usedMem;
+            if (print) System.out.printf("Memory usage: %.2fMB\n", usedMem);
+        }
+
+        boolean collectionThresholdPassed = usedMem >= collectionThreshold;
+        boolean collectionPeriodPassed =
+                collectionPeriodMillis != -1
+                        && (System.currentTimeMillis() - lastCollectionMillis >= collectionPeriodMillis);
+
+        if (collectionThresholdPassed || collectionPeriodPassed) {
+            collect();
+            lastCollectionMillis = System.currentTimeMillis();
+            if (print) {
+                System.out.printf("Garbage collected at %.2fMB\n", usedMem);
+            }
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/Platform.java

@@ -0,0 +1,106 @@
+package org.photonvision.common.util;
+
+import edu.wpi.first.wpiutil.RuntimeDetector;
+import java.io.IOException;
+
+@SuppressWarnings("unused")
+public enum Platform {
+    // WPILib Supported (JNI)
+    WINDOWS_32("Windows x32"),
+    WINDOWS_64("Windows x64"),
+    LINUX_64("Linux x64"),
+    LINUX_RASPBIAN("Linux Raspbian"), // Raspberry Pi 3/4
+    LINUX_AARCH64BIONIC("Linux AARCH64 Bionic"), // Jetson Nano, Jetson TX2
+    MACOS_64("Mac OS x64"),
+
+    // ChameleonVision Supported (Manual install)
+    LINUX_ARM32("Linux ARM32"), // ODROID XU4, C1+
+    LINUX_ARM64("Linux ARM64"), // ODROID C2, N2
+
+    // Completely unsupported
+    UNSUPPORTED("Unsupported Platform");
+
+    public final String value;
+    public final boolean isRoot = checkForRoot();
+
+    Platform(String value) {
+        this.value = value;
+    }
+
+    private static final String OS_NAME = System.getProperty("os.name");
+    private static final String OS_ARCH = System.getProperty("os.arch");
+    public static final Platform CurrentPlatform = getCurrentPlatform();
+
+    private static String UnknownPlatformString =
+            String.format("Unknown Platform. OS: %s, Architecture: %s", OS_NAME, OS_ARCH);
+
+    public boolean isWindows() {
+        return this == WINDOWS_64 || this == WINDOWS_32;
+    }
+
+    public boolean isLinux() {
+        return this == LINUX_64 || this == LINUX_RASPBIAN || this == LINUX_ARM64;
+    }
+
+    public boolean isMac() {
+        return this == MACOS_64;
+    }
+
+    public static boolean isRaspberryPi() {
+        return CurrentPlatform.equals(LINUX_RASPBIAN);
+    }
+
+    private static ShellExec shell = new ShellExec(true, false);
+
+    @SuppressWarnings("StatementWithEmptyBody")
+    private boolean checkForRoot() {
+        if (isLinux() || isMac()) {
+            try {
+                shell.execute("id", null, true, "-u");
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+
+            while (!shell.isOutputCompleted()) {
+                // TODO: add timeout
+            }
+
+            if (shell.getExitCode() == 0) {
+                return shell.getOutput().split("\n")[0].equals("0");
+            }
+
+        } else {
+            return true;
+        }
+        return false;
+    }
+
+    private static Platform getCurrentPlatform() {
+        if (RuntimeDetector.isWindows()) {
+            if (RuntimeDetector.is32BitIntel()) return WINDOWS_32;
+            if (RuntimeDetector.is64BitIntel()) return WINDOWS_64;
+        }
+
+        if (RuntimeDetector.isMac()) {
+            if (RuntimeDetector.is32BitIntel()) return UNSUPPORTED;
+            if (RuntimeDetector.is64BitIntel()) return MACOS_64;
+        }
+
+        if (RuntimeDetector.isLinux()) {
+            if (RuntimeDetector.is32BitIntel()) return UNSUPPORTED;
+            if (RuntimeDetector.is64BitIntel()) return LINUX_64;
+            if (RuntimeDetector.isRaspbian()) return LINUX_RASPBIAN;
+        }
+
+        System.out.println(UnknownPlatformString);
+        return Platform.UNSUPPORTED;
+    }
+
+    public String toString() {
+        if (this.equals(UNSUPPORTED)) {
+            return UnknownPlatformString;
+        } else {
+            return this.value;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/ReflectionUtils.java

@@ -0,0 +1,42 @@
+package org.photonvision.common.util;
+
+public class ReflectionUtils {
+
+    public static StackTraceElement[] getFullStackTrace() {
+        return Thread.currentThread().getStackTrace();
+    }
+
+    public static StackTraceElement getNthCaller(int n) {
+        if (n < 0) n = 0;
+        return Thread.currentThread().getStackTrace()[n];
+    }
+
+    public static String getCallerClassName() {
+        StackTraceElement[] stElements = Thread.currentThread().getStackTrace();
+        for (int i = 1; i < stElements.length; i++) {
+            StackTraceElement ste = stElements[i];
+            if (!ste.getClassName().equals(ReflectionUtils.class.getName())
+                    && ste.getClassName().indexOf("java.lang.Thread") != 0) {
+                return ste.getClassName();
+            }
+        }
+        return null;
+    }
+
+    public static String getCallerCallerClassName() {
+        StackTraceElement[] stElements = Thread.currentThread().getStackTrace();
+        String callerClassName = null;
+        for (int i = 1; i < stElements.length; i++) {
+            StackTraceElement ste = stElements[i];
+            if (!ste.getClassName().equals(ReflectionUtils.class.getName())
+                    && ste.getClassName().indexOf("java.lang.Thread") != 0) {
+                if (callerClassName == null) {
+                    callerClassName = ste.getClassName();
+                } else if (!callerClassName.equals(ste.getClassName())) {
+                    return ste.getClassName();
+                }
+            }
+        }
+        return null;
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/ShellExec.java

@@ -0,0 +1,179 @@
+package org.photonvision.common.util;
+
+import java.io.*;
+
+/** Execute external process and optionally read output buffer. */
+@SuppressWarnings({"unused", "ConstantConditions"})
+public class ShellExec {
+    private int exitCode;
+    private boolean readOutput, readError;
+    private StreamGobbler errorGobbler, outputGobbler;
+
+    public ShellExec() {
+        this(false, false);
+    }
+
+    public ShellExec(boolean readOutput, boolean readError) {
+        this.readOutput = readOutput;
+        this.readError = readError;
+    }
+
+    /**
+    * Execute a bash command. We can handle complex bash commands including multiple executions (; |
+    * && ||), quotes, expansions ($), escapes (\), e.g.: "cd /abc/def; mv ghi 'older ghi '$(whoami)"
+    *
+    * @param command Bash command to execute
+    * @return true if bash got started, but your command may have failed.
+    */
+    public int executeBashCommand(String command) throws IOException {
+        boolean wait = true;
+        boolean success = false;
+        Runtime r = Runtime.getRuntime();
+        // Use bash -c so we can handle things like multi commands separated by ; and
+        // things like quotes, $, |, and \. My tests show that command comes as
+        // one argument to bash, so we do not need to quote it to make it one thing.
+        // Also, exec may object if it does not have an executable file as the first thing,
+        // so having bash here makes it happy provided bash is installed and in path.
+        String[] commands = {"bash", "-c", command};
+
+        Process process = r.exec(commands);
+
+        // Consume streams, older jvm's had a memory leak if streams were not read,
+        // some other jvm+OS combinations may block unless streams are consumed.
+        return doProcess(wait, process);
+    }
+
+    /**
+    * Execute a command in current folder, and wait for process to end
+    *
+    * @param command command ("c:/some/folder/script.bat" or "some/folder/script.sh")
+    * @param args 0..n command line arguments
+    * @return process exit code
+    */
+    public int execute(String command, String... args) throws IOException {
+        return execute(command, null, true, args);
+    }
+
+    /**
+    * Execute a command.
+    *
+    * @param command command ("c:/some/folder/script.bat" or "some/folder/script.sh")
+    * @param workdir working directory or NULL to use command folder
+    * @param wait wait for process to end
+    * @param args 0..n command line arguments
+    * @return process exit code
+    */
+    public int execute(String command, String workdir, boolean wait, String... args)
+            throws IOException {
+        String[] cmdArr;
+        if (args != null && args.length > 0) {
+            cmdArr = new String[1 + args.length];
+            cmdArr[0] = command;
+            System.arraycopy(args, 0, cmdArr, 1, args.length);
+        } else {
+            cmdArr = new String[] {command};
+        }
+
+        ProcessBuilder pb = new ProcessBuilder(cmdArr);
+        File workingDir = (workdir == null ? new File(command).getParentFile() : new File(workdir));
+        pb.directory(workingDir);
+
+        Process process = pb.start();
+
+        // Consume streams, older jvm's had a memory leak if streams were not read,
+        // some other jvm+OS combinations may block unless streams are consumed.
+        return doProcess(wait, process);
+    }
+
+    private int doProcess(boolean wait, Process process) {
+        errorGobbler = new StreamGobbler(process.getErrorStream(), readError);
+        outputGobbler = new StreamGobbler(process.getInputStream(), readOutput);
+        errorGobbler.start();
+        outputGobbler.start();
+
+        exitCode = 0;
+        if (wait) {
+            try {
+                process.waitFor();
+                exitCode = process.exitValue();
+            } catch (InterruptedException ignored) {
+            }
+        }
+        return exitCode;
+    }
+
+    public int getExitCode() {
+        return exitCode;
+    }
+
+    public boolean isOutputCompleted() {
+        return (outputGobbler != null && outputGobbler.isCompleted());
+    }
+
+    public boolean isErrorCompleted() {
+        return (errorGobbler != null && errorGobbler.isCompleted());
+    }
+
+    public String getOutput() {
+        return (outputGobbler != null ? outputGobbler.getOutput() : null);
+    }
+
+    public String getError() {
+        return (errorGobbler != null ? errorGobbler.getOutput() : null);
+    }
+
+    // ********************************************
+    // ********************************************
+
+    /**
+    * StreamGobbler reads inputstream to "gobble" it. This is used by Executor class when running a
+    * commandline applications. Gobblers must read/purge INSTR and ERRSTR process streams.
+    * http://www.javaworld.com/javaworld/jw-12-2000/jw-1229-traps.html?page=4
+    */
+    @SuppressWarnings("WeakerAccess")
+    private static class StreamGobbler extends Thread {
+        private InputStream is;
+        private StringBuilder output;
+        private volatile boolean completed; // mark volatile to guarantee a thread safety
+
+        public StreamGobbler(InputStream is, boolean readStream) {
+            this.is = is;
+            this.output = (readStream ? new StringBuilder(256) : null);
+        }
+
+        public void run() {
+            completed = false;
+            try {
+                String NL = System.getProperty("line.separator", "\r\n");
+
+                InputStreamReader isr = new InputStreamReader(is);
+                BufferedReader br = new BufferedReader(isr);
+                String line;
+                while ((line = br.readLine()) != null) {
+                    if (output != null) output.append(line).append(NL);
+                }
+            } catch (IOException ex) {
+                // ex.printStackTrace();
+            }
+            completed = true;
+        }
+
+        /**
+        * Get inputstream buffer or null if stream was not consumed.
+        *
+        * @return Output stream
+        */
+        public String getOutput() {
+            return (output != null ? output.toString() : null);
+        }
+
+        /**
+        * Is input stream completed.
+        *
+        * @return if input stream is completed
+        */
+        public boolean isCompleted() {
+            return completed;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/TestUtils.java

@@ -0,0 +1,178 @@
+package org.photonvision.common.util;
+
+import edu.wpi.cscore.CameraServerCvJNI;
+import java.awt.*;
+import java.io.IOException;
+import java.nio.file.Path;
+import org.opencv.core.Mat;
+import org.opencv.highgui.HighGui;
+
+public class TestUtils {
+
+    @SuppressWarnings("unused")
+    public enum WPI2019Image {
+        kCargoAngledDark48in(1.2192),
+        kCargoSideStraightDark36in(0.9144),
+        kCargoSideStraightDark60in(1.524),
+        kCargoSideStraightDark72in(1.8288),
+        kCargoSideStraightPanelDark36in(0.9144),
+        kCargoStraightDark19in(0.4826),
+        kCargoStraightDark24in(0.6096),
+        kCargoStraightDark48in(1.2192),
+        kCargoStraightDark72in(1.8288),
+        kCargoStraightDark72in_HighRes(1.8288),
+        kCargoStraightDark90in(2.286);
+
+        public static double FOV = 68.5;
+
+        public final double distanceMeters;
+        public final Path path;
+
+        Path getPath() {
+            var filename = this.toString().substring(1);
+            return Path.of("2019", "WPI", filename + ".jpg");
+        }
+
+        WPI2019Image(double distanceMeters) {
+            this.distanceMeters = distanceMeters;
+            this.path = getPath();
+        }
+    }
+
+    @SuppressWarnings("unused")
+    public enum WPI2020Image {
+        kBlueGoal_060in_Center(1.524),
+        kBlueGoal_084in_Center(2.1336),
+        kBlueGoal_084in_Center_720p(2.1336),
+        kBlueGoal_108in_Center(2.7432),
+        kBlueGoal_132in_Center(3.3528),
+        kBlueGoal_156in_Center(3.9624),
+        kBlueGoal_180in_Center(4.572),
+        kBlueGoal_156in_Left(3.9624),
+        kBlueGoal_224in_Left(5.6896),
+        kBlueGoal_228in_ProtectedZone(5.7912),
+        kBlueGoal_330in_ProtectedZone(8.382),
+        kBlueGoal_Far_ProtectedZone(10.668), // TODO: find a more accurate distance
+        kRedLoading_016in_Down(0.4064),
+        kRedLoading_030in_Down(0.762),
+        kRedLoading_048in_Down(1.2192),
+        kRedLoading_048in(1.2192),
+        kRedLoading_060in(1.524),
+        kRedLoading_084in(2.1336),
+        kRedLoading_108in(2.7432);
+
+        public static double FOV = 68.5;
+
+        public final double distanceMeters;
+        public final Path path;
+
+        Path getPath() {
+            var filename = this.toString().substring(1).replace('_', '-');
+            return Path.of("2020", "WPI", filename + ".jpg");
+        }
+
+        WPI2020Image(double distanceMeters) {
+            this.distanceMeters = distanceMeters;
+            this.path = getPath();
+        }
+    }
+
+    public enum PolygonTestImages {
+        kPolygons;
+
+        public final Path path;
+
+        Path getPath() {
+            var filename = this.toString().substring(1).toLowerCase();
+            return Path.of("polygons", filename + ".png");
+        }
+
+        PolygonTestImages() {
+            this.path = getPath();
+        }
+    }
+
+    public enum PowercellTestImages {
+        kPowercell_test_1,
+        kPowercell_test_2,
+        kPowercell_test_3,
+        kPowercell_test_4,
+        kPowercell_test_5,
+        kPowercell_test_6;
+
+        public final Path path;
+
+        Path getPath() {
+            var filename = this.toString().substring(1).toLowerCase();
+            return Path.of(filename + ".png");
+        }
+
+        PowercellTestImages() {
+            this.path = getPath();
+        }
+    }
+
+    private static Path getResourcesFolderPath() {
+        return Path.of("src", "test", "resources").toAbsolutePath();
+    }
+
+    public static Path getTestImagesPath() {
+        return getResourcesFolderPath().resolve("testimages");
+    }
+
+    public static Path getCalibrationPath() {
+        return getResourcesFolderPath().resolve("calibration");
+    }
+
+    public static Path getPowercellPath() {
+        return getTestImagesPath().resolve("polygons").resolve("powercells");
+    }
+
+    public static Path getWPIImagePath(WPI2020Image image) {
+        return getTestImagesPath().resolve(image.path);
+    }
+
+    public static Path getWPIImagePath(WPI2019Image image) {
+        return getTestImagesPath().resolve(image.path);
+    }
+
+    public static Path getPolygonImagePath(PolygonTestImages image) {
+        return getTestImagesPath().resolve(image.path);
+    }
+
+    public static Path getPowercellImagePath(PowercellTestImages image) {
+        return getPowercellPath().resolve(image.path);
+    }
+
+    public static void loadLibraries() {
+        try {
+            CameraServerCvJNI.forceLoad();
+        } catch (IOException e) {
+            // ignored
+        }
+    }
+
+    private static int DefaultTimeoutMillis = 5000;
+
+    public static void showImage(Mat frame, String title, int timeoutMs) {
+        try {
+            HighGui.imshow(title, frame);
+            HighGui.waitKey(timeoutMs);
+            HighGui.destroyAllWindows();
+        } catch (HeadlessException ignored) {
+
+        }
+    }
+
+    public static void showImage(Mat frame, int timeoutMs) {
+        showImage(frame, "", timeoutMs);
+    }
+
+    public static void showImage(Mat frame, String title) {
+        showImage(frame, title, DefaultTimeoutMillis);
+    }
+
+    public static void showImage(Mat frame) {
+        showImage(frame, DefaultTimeoutMillis);
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/file/FileUtils.java

@@ -0,0 +1,56 @@
+package org.photonvision.common.util.file;
+
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.Platform;
+
+import java.io.File;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.attribute.PosixFileAttributes;
+import java.nio.file.attribute.PosixFilePermission;
+import java.util.Arrays;
+import java.util.HashSet;
+import java.util.Set;
+
+public class FileUtils {
+
+    private FileUtils() {}
+
+    private static Logger logger = new Logger(FileUtils.class, LogGroup.General);
+    private static final Set<PosixFilePermission> allReadWriteExecutePerms =
+            new HashSet<>(Arrays.asList(PosixFilePermission.values()));
+
+    public static void setFilePerms(Path path) throws IOException {
+        if (!Platform.CurrentPlatform.isWindows()) {
+            File thisFile = path.toFile();
+            Set<PosixFilePermission> perms =
+                    Files.readAttributes(path, PosixFileAttributes.class).permissions();
+            if (!perms.equals(allReadWriteExecutePerms)) {
+                logger.info("Setting perms on" + path.toString());
+                Files.setPosixFilePermissions(path, perms);
+                if (thisFile.isDirectory()) {
+                    for (File subfile : thisFile.listFiles()) {
+                        setFilePerms(subfile.toPath());
+                    }
+                }
+            }
+        }
+    }
+
+    public static void setAllPerms(Path path) {
+        if (!Platform.CurrentPlatform.isWindows()) {
+            String command = String.format("chmod 777 -R %s", path.toString());
+            try {
+                Process p = Runtime.getRuntime().exec(command);
+                p.waitFor();
+
+            } catch (Exception e) {
+                e.printStackTrace();
+            }
+        } else {
+            logger.info("Cannot set directory permissions on Windows!");
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/file/JacksonUtils.java

@@ -0,0 +1,86 @@
+package org.photonvision.common.util.file;
+
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.json.JsonMapper;
+import com.fasterxml.jackson.databind.jsontype.BasicPolymorphicTypeValidator;
+import com.fasterxml.jackson.databind.jsontype.PolymorphicTypeValidator;
+import com.fasterxml.jackson.databind.module.SimpleModule;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+import java.io.File;
+import java.io.FileDescriptor;
+import java.io.FileOutputStream;
+import java.io.IOException;
+import java.nio.file.Path;
+
+public class JacksonUtils {
+    public static <T> void serializer(Path path, T object) throws IOException {
+        serializer(path, object, false);
+    }
+
+    public static <T> void serializer(Path path, T object, boolean forceSync) throws IOException {
+        PolymorphicTypeValidator ptv =
+                BasicPolymorphicTypeValidator.builder().allowIfBaseType(object.getClass()).build();
+        ObjectMapper objectMapper =
+                JsonMapper.builder()
+                        .activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT)
+                        .build();
+        String json = objectMapper.writerWithDefaultPrettyPrinter().writeValueAsString(object);
+        saveJsonString(json, path, forceSync);
+    }
+
+    public static <T> T deserialize(Path path, Class<T> ref) throws IOException {
+        PolymorphicTypeValidator ptv =
+                BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();
+        ObjectMapper objectMapper =
+                JsonMapper.builder()
+                        .activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT)
+                        .build();
+        File jsonFile = new File(path.toString());
+        if (jsonFile.exists() && jsonFile.length() > 0) {
+            return objectMapper.readValue(jsonFile, ref);
+        }
+        return null;
+    }
+
+    public static <T> T deserialize(Path path, Class<T> ref, StdDeserializer<T> deserializer)
+            throws IOException {
+        ObjectMapper objectMapper = new ObjectMapper();
+        SimpleModule module = new SimpleModule();
+        module.addDeserializer(ref, deserializer);
+        objectMapper.registerModule(module);
+
+        File jsonFile = new File(path.toString());
+        if (jsonFile.exists() && jsonFile.length() > 0) {
+            return objectMapper.readValue(jsonFile, ref);
+        }
+        return null;
+    }
+
+    public static <T> void serialize(Path path, T object, Class<T> ref, StdSerializer<T> serializer)
+            throws IOException {
+        serialize(path, object, ref, serializer, false);
+    }
+
+    public static <T> void serialize(
+            Path path, T object, Class<T> ref, StdSerializer<T> serializer, boolean forceSync)
+            throws IOException {
+        ObjectMapper objectMapper = new ObjectMapper();
+        SimpleModule module = new SimpleModule();
+        module.addSerializer(ref, serializer);
+        objectMapper.registerModule(module);
+        String json = objectMapper.writerWithDefaultPrettyPrinter().writeValueAsString(object);
+        saveJsonString(json, path, forceSync);
+    }
+
+    private static void saveJsonString(String json, Path path, boolean forceSync) throws IOException {
+        FileOutputStream fileOutputStream = new FileOutputStream(path.toFile());
+        fileOutputStream.write(json.getBytes());
+        fileOutputStream.flush();
+        if (forceSync) {
+            FileDescriptor fileDescriptor = fileOutputStream.getFD();
+            fileDescriptor.sync();
+        }
+        fileOutputStream.close();
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/math/IPUtils.java

@@ -0,0 +1,39 @@
+package org.photonvision.common.util.math;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class IPUtils {
+    public static boolean isValidIPV4(final String ip) {
+        String PATTERN =
+                "^((0|1\\d?\\d?|2[0-4]?\\d?|25[0-5]?|[3-9]\\d?)\\.){3}(0|1\\d?\\d?|2[0-4]?\\d?|25[0-5]?|[3-9]\\d?)$";
+
+        return ip.matches(PATTERN);
+    }
+
+    public static List<Byte> getDigitBytes(int num) {
+        List<Byte> digits = new ArrayList<>();
+        collectDigitBytes(num, digits);
+        return digits;
+    }
+
+    private static void collectDigitBytes(int num, List<Byte> digits) {
+        if (num / 10 > 0) {
+            collectDigitBytes(num / 10, digits);
+        }
+        digits.add((byte) (num % 10));
+    }
+
+    public static List<Integer> getDigits(int num) {
+        List<Integer> digits = new ArrayList<>();
+        collectDigits(num, digits);
+        return digits;
+    }
+
+    private static void collectDigits(int num, List<Integer> digits) {
+        if (num / 10 > 0) {
+            collectDigits(num / 10, digits);
+        }
+        digits.add(num % 10);
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java

@@ -0,0 +1,33 @@
+package org.photonvision.common.util.math;
+
+import org.apache.commons.math3.util.FastMath;
+
+public class MathUtils {
+    MathUtils() {}
+
+    public static double sigmoid(Number x) {
+        double bias = 0;
+        double a = 5;
+        double b = -0.05;
+        double k = 200;
+
+        if (x.doubleValue() < 50) {
+            bias = -1.338;
+        }
+
+        return ((k / (1 + Math.pow(Math.E, (a + (b * x.doubleValue()))))) + bias);
+    }
+
+    public static double toSlope(Number angle) {
+        return FastMath.atan(FastMath.toRadians(angle.doubleValue() - 90));
+    }
+
+    public static double roundTo(double value, int to) {
+        double toMult = Math.pow(10, to);
+        return (double) Math.round(value * toMult) / toMult;
+    }
+
+    public static double nanosToMillis(long nanos) {
+        return nanos / 1000000.0;
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/numbers/DoubleCouple.java

@@ -0,0 +1,27 @@
+package org.photonvision.common.util.numbers;
+
+import org.opencv.core.Point;
+
+public class DoubleCouple extends NumberCouple<Double> {
+
+    public DoubleCouple() {
+        super(0.0, 0.0);
+    }
+
+    public DoubleCouple(Double first, Double second) {
+        super(first, second);
+    }
+
+    public DoubleCouple(Point point) {
+        super(point.x, point.y);
+    }
+
+    public Point toPoint() {
+        return new Point(first, second);
+    }
+
+    public void fromPoint(Point point) {
+        first = point.x;
+        second = point.y;
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/numbers/IntegerCouple.java

@@ -0,0 +1,12 @@
+package org.photonvision.common.util.numbers;
+
+public class IntegerCouple extends NumberCouple<Integer> {
+
+    public IntegerCouple() {
+        super(0, 0);
+    }
+
+    public IntegerCouple(Integer first, Integer second) {
+        super(first, second);
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/numbers/NumberCouple.java

@@ -0,0 +1,58 @@
+package org.photonvision.common.util.numbers;
+
+import com.fasterxml.jackson.annotation.JsonIgnore;
+
+public abstract class NumberCouple<T extends Number> {
+
+    protected T first;
+    protected T second;
+
+    public NumberCouple(T first, T second) {
+        this.first = first;
+        this.second = second;
+    }
+
+    public void setFirst(T first) {
+        this.first = first;
+    }
+
+    public T getFirst() {
+        return first;
+    }
+
+    public void setSecond(T second) {
+        this.second = second;
+    }
+
+    public T getSecond() {
+        return second;
+    }
+
+    public void set(T first, T second) {
+        this.first = first;
+        this.second = second;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (!(obj instanceof NumberCouple)) {
+            return false;
+        }
+
+        var couple = (NumberCouple) obj;
+        if (!couple.first.equals(first)) {
+            return false;
+        }
+
+        if (!couple.second.equals(second)) {
+            return false;
+        }
+
+        return true;
+    }
+
+    @JsonIgnore
+    public boolean isEmpty() {
+        return first.intValue() == 0 && second.intValue() == 0;
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/numbers/NumberListUtils.java

@@ -0,0 +1,98 @@
+package org.photonvision.common.util.numbers;
+
+import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+import java.util.StringJoiner;
+
+@SuppressWarnings("unused")
+public class NumberListUtils {
+
+    /**
+    * @param collection an ArrayList of Comparable objects
+    * @return the median of collection
+    */
+    public static <T extends Number> double median(List<T> collection, Comparator<T> comp) {
+        double result;
+        int n = collection.size() / 2;
+
+        if (collection.size() % 2 == 0) // even number of items; find the middle two and average them
+        result =
+                    (nthSmallest(collection, n - 1, comp).doubleValue()
+                                    + nthSmallest(collection, n, comp).doubleValue())
+                            / 2.0;
+        else // odd number of items; return the one in the middle
+        result = nthSmallest(collection, n, comp).doubleValue();
+
+        return result;
+    }
+
+    public static <T extends Number> String toString(List<T> collection) {
+        return toString(collection, "");
+    }
+
+    public static <T extends Number> String toString(List<T> collection, String suffix) {
+        StringJoiner joiner = new StringJoiner(", ");
+        for (T x : collection) {
+            String s = x.doubleValue() + suffix;
+            joiner.add(s);
+        }
+        return joiner.toString();
+    }
+
+    /**
+    * @param collection an ArrayList of Numbers
+    * @return the mean of collection
+    */
+    public static double mean(final List<? extends Number> collection) {
+        BigDecimal sum = BigDecimal.ZERO;
+        for (final Number number : collection) {
+            sum = sum.add(BigDecimal.valueOf(number.doubleValue()));
+        }
+        return (sum.doubleValue() / collection.size());
+    }
+
+    /**
+    * @param collection a collection of Comparable objects
+    * @param n the position of the desired object, using the ordering defined on the collection
+    *     elements
+    * @return the nth smallest object
+    */
+    public static <T> T nthSmallest(List<T> collection, int n, Comparator<T> comp) {
+        T result, pivot;
+        ArrayList<T> underPivot = new ArrayList<>(),
+                overPivot = new ArrayList<>(),
+                equalPivot = new ArrayList<>();
+
+        // choosing a pivot is a whole topic in itself.
+        // this implementation uses the simple strategy of grabbing something from the middle of the
+        // ArrayList.
+
+        pivot = collection.get(n / 2);
+
+        // split collection into 3 lists based on comparison with the pivot
+
+        for (T obj : collection) {
+            int order = comp.compare(obj, pivot);
+
+            if (order < 0) // obj < pivot
+            underPivot.add(obj);
+            else if (order > 0) // obj > pivot
+            overPivot.add(obj);
+            else // obj = pivot
+            equalPivot.add(obj);
+        } // for each obj in collection
+
+        // recurse on the appropriate collection
+
+        if (n < underPivot.size()) result = nthSmallest(underPivot, n, comp);
+        else if (n < underPivot.size() + equalPivot.size()) // equal to pivot; just return it
+        result = pivot;
+        else // everything in underPivot and equalPivot is too small.  Adjust n accordingly in the
+            // recursion.
+            result = nthSmallest(overPivot, n - underPivot.size() - equalPivot.size(), comp);
+
+        return result;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/camera/CameraQuirks.java

@@ -0,0 +1,5 @@
+package org.photonvision.common.vision.camera;
+
+public enum CameraQuirks {
+    Gain
+}

photon-server/src/main/java/org/photonvision/common/vision/camera/CameraType.java

@@ -0,0 +1,6 @@
+package org.photonvision.common.vision.camera;
+
+public enum CameraType {
+    UsbCamera,
+    HttpCamera
+}

photon-server/src/main/java/org/photonvision/common/vision/camera/QuirkyCamera.java

@@ -0,0 +1,35 @@
+package org.photonvision.common.vision.camera;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class QuirkyCamera {
+    public static final List<QuirkyCamera> quirkyCameras =
+            List.of(
+                    // ps3 eye
+                    new QuirkyCamera(0x1415, 0x2000, "PS3Eye", List.of(CameraQuirks.Gain)));
+
+    public final int usbVid;
+    public final int usbPid;
+    public final String name;
+    public List<CameraQuirks> quirks;
+
+    public QuirkyCamera(int usbVid, int usbPid, String baseName, List<CameraQuirks> quirks) {
+        this.usbVid = usbVid;
+        this.usbPid = usbPid;
+        this.name = baseName;
+        this.quirks = quirks;
+    }
+
+    public QuirkyCamera(int usbVid, int usbPid, String baseName) {
+        this(usbVid, usbPid, baseName, new ArrayList<>());
+        QuirkyCamera quirky =
+                quirkyCameras.stream()
+                        .filter(quirkyCamera -> quirkyCamera.usbPid == usbPid && quirkyCamera.usbVid == usbVid)
+                        .findFirst()
+                        .orElse(null);
+        if (quirky != null) {
+            this.quirks = quirky.quirks;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/camera/USBCameraSource.java

@@ -0,0 +1,123 @@
+package org.photonvision.common.vision.camera;
+
+import org.photonvision.common.configuration.CameraConfiguration;
+import org.photonvision.common.vision.frame.FrameProvider;
+import org.photonvision.common.vision.frame.provider.USBFrameProvider;
+import org.photonvision.common.vision.processes.VisionSource;
+import org.photonvision.common.vision.processes.VisionSourceSettables;
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.cameraserver.CameraServer;
+
+import java.util.*;
+
+public class USBCameraSource implements VisionSource {
+    private final UsbCamera camera;
+    private final USBCameraSettables usbCameraSettables;
+    private final USBFrameProvider usbFrameProvider;
+    private final CameraConfiguration configuration;
+
+    private final QuirkyCamera cameraQuirks;
+
+    public USBCameraSource(CameraConfiguration config) {
+        this.configuration = config;
+        this.camera = new UsbCamera(config.nickname, config.path);
+        this.cameraQuirks =
+                new QuirkyCamera(camera.getInfo().productId, camera.getInfo().vendorId, config.baseName);
+        CvSink cvSink = CameraServer.getInstance().getVideo(this.camera);
+        this.usbCameraSettables = new USBCameraSettables(config);
+        this.usbFrameProvider =
+                new USBFrameProvider(cvSink, usbCameraSettables.getFrameStaticProperties());
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (obj.getClass() != USBCameraSource.class) {
+            return false;
+        }
+        USBCameraSource tmp = (USBCameraSource) obj;
+        boolean i = this.cameraQuirks.quirks.equals(tmp.cameraQuirks.quirks);
+
+        boolean r = this.configuration.uniqueName.equals(tmp.configuration.uniqueName);
+        boolean c = this.configuration.baseName.equals(tmp.configuration.baseName);
+        boolean j = this.configuration.nickname.equals(tmp.configuration.nickname);
+
+        boolean k = this.camera.getInfo().name.equals(tmp.camera.getInfo().name);
+        boolean x = this.camera.getInfo().productId == tmp.camera.getInfo().productId;
+        boolean y = this.camera.getInfo().vendorId == tmp.camera.getInfo().vendorId;
+        var t = i && r && c && j && k && x && y;
+        return t;
+    }
+
+    @Override
+    public FrameProvider getFrameProvider() {
+        return usbFrameProvider;
+    }
+
+    @Override
+    public VisionSourceSettables getSettables() {
+        return this.usbCameraSettables;
+    }
+
+    public class USBCameraSettables extends VisionSourceSettables {
+        protected USBCameraSettables(CameraConfiguration configuration) {
+            super(configuration);
+        }
+
+        @Override
+        public int getExposure() {
+            return camera.getProperty("exposure").get();
+        }
+
+        @Override
+        public void setExposure(int exposure) {
+            camera.setExposureManual(exposure);
+        }
+
+        @Override
+        public int getBrightness() {
+            return camera.getBrightness();
+        }
+
+        @Override
+        public void setBrightness(int brightness) {
+            camera.setBrightness(brightness);
+        }
+
+        @Override
+        public int getGain() {
+            return camera.getProperty("gain").get();
+        }
+
+        @Override
+        public void setGain(int gain) {
+            if (cameraQuirks.quirks.contains(CameraQuirks.Gain)) {
+                camera.getProperty("gain_automatic").set(0);
+                camera.getProperty("gain").set(gain);
+            }
+        }
+
+        @Override
+        public VideoMode getCurrentVideoMode() {
+            return camera.getVideoMode();
+        }
+
+        @Override
+        public void setCurrentVideoMode(VideoMode videoMode) {
+            camera.setVideoMode(videoMode);
+        }
+
+        @Override
+        public HashMap<Integer, VideoMode> getAllVideoModes() {
+            if (videoModes == null) {
+                videoModes = new HashMap<>();
+                List<VideoMode> videoModesList = Arrays.asList(camera.enumerateVideoModes());
+                for (VideoMode videoMode : videoModesList) {
+                    videoModes.put(videoModesList.indexOf(videoMode), videoMode);
+                }
+            }
+            return videoModes;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/frame/Frame.java

@@ -0,0 +1,37 @@
+package org.photonvision.common.vision.frame;
+
+import org.photonvision.common.vision.opencv.CVMat;
+import org.photonvision.common.vision.opencv.Releasable;
+import org.opencv.core.Mat;
+
+public class Frame implements Releasable {
+    public final long timestampNanos;
+    public final CVMat image;
+    public final FrameStaticProperties frameStaticProperties;
+
+    public Frame(CVMat image, long timestampNanos, FrameStaticProperties frameStaticProperties) {
+        this.image = image;
+        this.timestampNanos = timestampNanos;
+        this.frameStaticProperties = frameStaticProperties;
+    }
+
+    public Frame(CVMat image, FrameStaticProperties frameStaticProperties) {
+        this(image, System.nanoTime(), frameStaticProperties);
+    }
+
+    public void copyTo(Mat destMat) {
+        image.getMat().copyTo(destMat);
+    }
+
+    public static Frame copyFrom(Frame frame) {
+        Mat newMat = new Mat();
+        frame.image.getMat().copyTo(newMat);
+        frame.release();
+        return new Frame(new CVMat(newMat), frame.timestampNanos, frame.frameStaticProperties);
+    }
+
+    @Override
+    public void release() {
+        image.release();
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/frame/FrameConsumer.java

@@ -0,0 +1,5 @@
+package org.photonvision.common.vision.frame;
+
+import java.util.function.Consumer;
+
+public interface FrameConsumer extends Consumer<Frame> {}

photon-server/src/main/java/org/photonvision/common/vision/frame/FrameDivisor.java

@@ -0,0 +1,14 @@
+package org.photonvision.common.vision.frame;
+
+public enum FrameDivisor {
+    NONE(1),
+    HALF(2),
+    QUARTER(4),
+    SIXTH(6);
+
+    public final Integer value;
+
+    FrameDivisor(int value) {
+        this.value = value;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/frame/FrameProvider.java

@@ -0,0 +1,7 @@
+package org.photonvision.common.vision.frame;
+
+import java.util.function.Supplier;
+
+public interface FrameProvider extends Supplier<Frame> {
+    String getName();
+}

photon-server/src/main/java/org/photonvision/common/vision/frame/FrameStaticProperties.java

@@ -0,0 +1,64 @@
+package org.photonvision.common.vision.frame;
+
+import edu.wpi.cscore.VideoMode;
+import org.apache.commons.math3.fraction.Fraction;
+import org.apache.commons.math3.util.FastMath;
+import org.opencv.core.Point;
+
+/** Represents the properties of a frame. */
+public class FrameStaticProperties {
+    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 Point centerPoint;
+    public final double horizontalFocalLength;
+    public final double verticalFocalLength;
+
+    /**
+    * Instantiates a new Frame static properties.
+    *
+    * @param mode The Video Mode of the camera.
+    * @param fov The fov of the image.
+    */
+    public FrameStaticProperties(VideoMode mode, double fov) {
+        this(mode.width, mode.height, fov);
+    }
+
+    /**
+    * Instantiates a new Frame static properties.
+    *
+    * @param imageWidth The width of the image.
+    * @param imageHeight The width of the image.
+    * @param fov The fov of the image.
+    */
+    public FrameStaticProperties(int imageWidth, int imageHeight, double fov) {
+        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;
+        centerPoint = new Point(centerX, centerY);
+
+        // 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));
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/frame/consumer/DummyFrameConsumer.java

@@ -0,0 +1,11 @@
+package org.photonvision.common.vision.frame.consumer;
+
+import org.photonvision.common.vision.frame.Frame;
+import org.photonvision.common.vision.frame.FrameConsumer;
+
+public class DummyFrameConsumer implements FrameConsumer {
+    @Override
+    public void accept(Frame frame) {
+        frame.release(); // lol ez
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/frame/consumer/MJPGFrameConsumer.java

@@ -0,0 +1,12 @@
+package org.photonvision.common.vision.frame.consumer;
+
+import org.photonvision.common.vision.frame.Frame;
+import org.photonvision.common.vision.frame.FrameConsumer;
+import org.apache.commons.lang3.NotImplementedException;
+
+public class MJPGFrameConsumer implements FrameConsumer {
+    @Override
+    public void accept(Frame frame) {
+        throw new NotImplementedException("");
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/frame/provider/FileFrameProvider.java

@@ -0,0 +1,98 @@
+package org.photonvision.common.vision.frame.provider;
+
+import org.photonvision.common.vision.frame.Frame;
+import org.photonvision.common.vision.frame.FrameProvider;
+import org.photonvision.common.vision.frame.FrameStaticProperties;
+import org.photonvision.common.vision.opencv.CVMat;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import org.opencv.core.Mat;
+import org.opencv.imgcodecs.Imgcodecs;
+
+/**
+* A {@link FrameProvider} that will read and provide an image from a {@link java.nio.file.Path
+* path}.
+*/
+public class FileFrameProvider implements FrameProvider {
+    private static int count = 0;
+
+    private Frame m_frame;
+    private final Path m_path;
+
+    private final double m_fov;
+
+    private boolean m_reloadImage;
+
+    /**
+    * Instantiates a new FileFrameProvider.
+    *
+    * @param path The path of the image to read from.
+    * @param fov The fov of the image.
+    */
+    public FileFrameProvider(Path path, double fov) {
+        if (!Files.exists(path))
+            throw new RuntimeException("Invalid path for image: " + path.toAbsolutePath().toString());
+        m_path = path;
+        m_fov = fov;
+
+        loadImage();
+    }
+
+    /**
+    * Instantiates a new File frame provider.
+    *
+    * @param pathAsString The path of the image to read from as a string.
+    * @param fov The fov of the image.
+    */
+    public FileFrameProvider(String pathAsString, double fov) {
+        this(Paths.get(pathAsString), fov);
+    }
+
+    private void loadImage() {
+        Mat image = Imgcodecs.imread(m_path.toString());
+
+        if (image.cols() > 0 && image.rows() > 0) {
+            FrameStaticProperties m_properties =
+                    new FrameStaticProperties(image.width(), image.height(), m_fov);
+            m_frame = new Frame(new CVMat(image), m_properties);
+        } else {
+            throw new RuntimeException("Image loading failed!");
+        }
+    }
+
+    /**
+    * Set image reloading. If true this will reload the image from the path set in the constructor
+    * every time {@link FileFrameProvider#get()} is called.
+    *
+    * @param reloadImage True to enable image reloading.
+    */
+    public void setImageReloading(boolean reloadImage) {
+        m_reloadImage = reloadImage;
+    }
+
+    /**
+    * Returns if image reloading is enabled.
+    *
+    * @return True if image reloading is enabled.
+    */
+    public boolean isImageReloading() {
+        return m_reloadImage;
+    }
+
+    @Override
+    public Frame get() {
+        if (m_reloadImage) {
+            if (m_frame != null) m_frame.release();
+            m_frame = null;
+            loadImage();
+        }
+
+        return m_frame;
+    }
+
+    @Override
+    public String getName() {
+        return "FileFrameProvider" + count++ + " - " + m_path.getFileName();
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/frame/provider/NetworkFrameProvider.java

@@ -0,0 +1,19 @@
+package org.photonvision.common.vision.frame.provider;
+
+import org.photonvision.common.vision.frame.Frame;
+import org.photonvision.common.vision.frame.FrameProvider;
+import org.apache.commons.lang3.NotImplementedException;
+
+public class NetworkFrameProvider implements FrameProvider {
+    private int count = 0;
+
+    @Override
+    public Frame get() {
+        throw new NotImplementedException("");
+    }
+
+    @Override
+    public String getName() {
+        return "NetworkFrameProvider" + count++;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/frame/provider/USBFrameProvider.java

@@ -0,0 +1,34 @@
+package org.photonvision.common.vision.frame.provider;
+
+import org.photonvision.common.vision.frame.Frame;
+import org.photonvision.common.vision.frame.FrameProvider;
+import org.photonvision.common.vision.frame.FrameStaticProperties;
+import org.photonvision.common.vision.opencv.CVMat;
+import edu.wpi.cscore.CvSink;
+
+public class USBFrameProvider implements FrameProvider {
+    private static int count = 0;
+    private CvSink cvSink;
+    private FrameStaticProperties frameStaticProperties;
+    private CVMat mat;
+
+    public USBFrameProvider(CvSink sink, FrameStaticProperties frameStaticProperties) {
+        cvSink = sink;
+        this.frameStaticProperties = frameStaticProperties;
+        mat = new CVMat();
+    }
+
+    @Override
+    public Frame get() {
+        if (mat != null && mat.getMat() != null) {
+            mat.release();
+        }
+        long time = cvSink.grabFrame(mat.getMat());
+        return new Frame(mat, time, frameStaticProperties);
+    }
+
+    @Override
+    public String getName() {
+        return "USBFrameProvider" + count++;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/opencv/CVMat.java

@@ -0,0 +1,54 @@
+package org.photonvision.common.vision.opencv;
+
+import org.photonvision.common.util.ReflectionUtils;
+import java.util.HashSet;
+import org.opencv.core.Mat;
+
+public class CVMat implements Releasable {
+    private static final HashSet<Mat> allMats = new HashSet<>();
+
+    private static boolean shouldPrint;
+
+    private final Mat mat;
+
+    public CVMat() {
+        this.mat = new Mat();
+    }
+
+    public void copyTo(CVMat srcMat) {
+        copyTo(srcMat.getMat());
+    }
+
+    public void copyTo(Mat srcMat) {
+        srcMat.copyTo(mat);
+    }
+
+    public CVMat(Mat mat) {
+        this.mat = mat;
+        if (allMats.add(mat) && shouldPrint) {
+            System.out.println(
+                    "(CVMat) Added new Mat (count: "
+                            + allMats.size()
+                            + ") from: "
+                            + ReflectionUtils.getNthCaller(3));
+        }
+    }
+
+    @Override
+    public void release() {
+        allMats.remove(mat);
+        mat.release();
+    }
+
+    public Mat getMat() {
+        return mat;
+    }
+
+    public static int getMatCount() {
+        return allMats.size();
+    }
+
+    public static void enablePrint(boolean enabled) {
+        shouldPrint = enabled;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/opencv/CVShape.java

@@ -0,0 +1,62 @@
+package org.photonvision.common.vision.opencv;
+
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.imgproc.Imgproc;
+
+public class CVShape {
+    public final Contour contour;
+    public final ContourShape shape;
+
+    private MatOfPoint3f customTarget = null;
+
+    private MatOfPoint2f approxCurve = new MatOfPoint2f();
+
+    public CVShape(Contour contour, ContourShape shape) {
+        this.contour = contour;
+        this.shape = shape;
+    }
+
+    public CVShape(Contour contour, MatOfPoint3f targetPoints) {
+        this.contour = contour;
+        this.shape = ContourShape.Custom;
+        customTarget = targetPoints;
+    }
+
+    public Contour getContour() {
+        return contour;
+    }
+
+    public MatOfPoint2f getApproxPolyDp(double epsilon, boolean closed) {
+        approxCurve.release();
+        approxCurve = new MatOfPoint2f();
+
+        Imgproc.approxPolyDP(contour.getMat2f(), approxCurve, epsilon, closed);
+        return approxCurve;
+    }
+
+    public MatOfPoint2f getApproxPolyDpConvex(double epsilon, boolean closed) {
+        approxCurve.release();
+        approxCurve = new MatOfPoint2f();
+
+        Imgproc.approxPolyDP(contour.getConvexHull(), approxCurve, epsilon, closed);
+        return approxCurve;
+    }
+
+    boolean approxPolyMatchesShape() {
+        var pointList = approxCurve.toList();
+
+        // TODO: @Matt
+        switch (shape) {
+            case Custom:
+                break;
+            case Circle:
+                break;
+            case Triangle:
+                break;
+            case Quadrilateral:
+                break;
+        }
+        return true;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/opencv/Contour.java

@@ -0,0 +1,198 @@
+package org.photonvision.common.vision.opencv;
+
+import org.photonvision.common.util.math.MathUtils;
+import java.util.Comparator;
+import org.opencv.core.CvType;
+import org.opencv.core.MatOfInt;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.RotatedRect;
+import org.opencv.imgproc.Imgproc;
+import org.opencv.imgproc.Moments;
+
+public class Contour implements Releasable {
+
+    public static final Comparator<Contour> SortByMomentsX =
+            Comparator.comparingDouble(
+                    (contour) -> contour.getMoments().get_m10() / contour.getMoments().get_m00());
+
+    public final MatOfPoint mat;
+
+    private Double area = Double.NaN;
+    private Double perimeter = Double.NaN;
+    private MatOfPoint2f mat2f = null;
+    private RotatedRect minAreaRect = null;
+    private Rect boundingRect = null;
+    private Moments moments = null;
+
+    private MatOfPoint2f convexHull = null;
+
+    public Contour(MatOfPoint mat) {
+        this.mat = mat;
+    }
+
+    public MatOfPoint2f getMat2f() {
+        if (mat2f == null) {
+            mat2f = new MatOfPoint2f(mat.toArray());
+            mat.convertTo(mat2f, CvType.CV_32F);
+        }
+        return mat2f;
+    }
+
+    public MatOfPoint2f getConvexHull() {
+        if (this.convexHull == null) {
+            var ints = new MatOfInt();
+            Imgproc.convexHull(mat, ints);
+            this.convexHull = Contour.convertIndexesToPoints(mat, ints);
+            ints.release();
+        }
+        return convexHull;
+    }
+
+    public double getArea() {
+        if (Double.isNaN(area)) {
+            area = Imgproc.contourArea(mat);
+        }
+        return area;
+    }
+
+    public double getPerimeter() {
+        if (Double.isNaN(perimeter)) {
+            perimeter = Imgproc.arcLength(getMat2f(), true);
+        }
+        return perimeter;
+    }
+
+    public RotatedRect getMinAreaRect() {
+        if (minAreaRect == null) {
+            minAreaRect = Imgproc.minAreaRect(getMat2f());
+        }
+        return minAreaRect;
+    }
+
+    public Rect getBoundingRect() {
+        if (boundingRect == null) {
+            boundingRect = Imgproc.boundingRect(mat);
+        }
+        return boundingRect;
+    }
+
+    public Moments getMoments() {
+        if (moments == null) {
+            moments = Imgproc.moments(mat);
+        }
+        return moments;
+    }
+
+    public Point getCenterPoint() {
+        return getMinAreaRect().center;
+    }
+
+    public boolean isEmpty() {
+        return mat.empty();
+    }
+
+    public boolean isIntersecting(
+            Contour secondContour, ContourIntersectionDirection intersectionDirection) {
+        boolean isIntersecting = false;
+
+        if (intersectionDirection == ContourIntersectionDirection.None) {
+            isIntersecting = true;
+        } else {
+            try {
+                MatOfPoint2f intersectMatA = new MatOfPoint2f();
+                MatOfPoint2f intersectMatB = new MatOfPoint2f();
+
+                mat.convertTo(intersectMatA, CvType.CV_32F);
+                secondContour.mat.convertTo(intersectMatB, CvType.CV_32F);
+
+                RotatedRect a = Imgproc.fitEllipse(intersectMatA);
+                RotatedRect b = Imgproc.fitEllipse(intersectMatB);
+                double mA = MathUtils.toSlope(a.angle);
+                double mB = MathUtils.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 (intersectionDirection) {
+                    case Up:
+                        if (intersectionY < massY) isIntersecting = true;
+                        break;
+                    case Down:
+                        if (intersectionY > massY) isIntersecting = true;
+                        break;
+                    case Left:
+                        if (intersectionX < massX) isIntersecting = true;
+                        break;
+                    case Right:
+                        if (intersectionX > massX) isIntersecting = true;
+                        break;
+                }
+                intersectMatA.release();
+                intersectMatB.release();
+            } catch (Exception e) {
+                // defaults to false
+            }
+        }
+
+        return isIntersecting;
+    }
+
+    // TODO: refactor to do "infinite" contours ???????
+    public static Contour groupContoursByIntersection(
+            Contour firstContour, Contour secondContour, ContourIntersectionDirection intersection) {
+        if (areIntersecting(firstContour, secondContour, intersection)) {
+            return combineContours(firstContour, secondContour);
+        } else {
+            return null;
+        }
+    }
+
+    public static boolean areIntersecting(
+            Contour firstContour,
+            Contour secondContour,
+            ContourIntersectionDirection intersectionDirection) {
+        return firstContour.isIntersecting(secondContour, intersectionDirection)
+                || secondContour.isIntersecting(firstContour, intersectionDirection);
+    }
+
+    private static Contour combineContours(Contour... contours) {
+        var points = new MatOfPoint();
+
+        for (var contour : contours) {
+            points.push_back(contour.mat);
+        }
+
+        var finalContour = new Contour(points);
+
+        boolean contourEmpty = finalContour.isEmpty();
+        return contourEmpty ? null : finalContour;
+    }
+
+    @Override
+    public void release() {
+        if (mat != null) mat.release();
+        if (mat2f != null) mat2f.release();
+        if (convexHull != null) convexHull.release();
+    }
+
+    public static MatOfPoint2f convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) {
+        int[] arrIndex = indexes.toArray();
+        Point[] arrContour = contour.toArray();
+        Point[] arrPoints = new Point[arrIndex.length];
+
+        for (int i = 0; i < arrIndex.length; i++) {
+            arrPoints[i] = arrContour[arrIndex[i]];
+        }
+
+        var hull = new MatOfPoint2f();
+        hull.fromArray(arrPoints);
+        return hull;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/opencv/ContourGroupingMode.java

@@ -0,0 +1,12 @@
+package org.photonvision.common.vision.opencv;
+
+public enum ContourGroupingMode {
+    Single(1),
+    Dual(2);
+
+    public final int count;
+
+    ContourGroupingMode(int count) {
+        this.count = count;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/opencv/ContourIntersectionDirection.java

@@ -0,0 +1,9 @@
+package org.photonvision.common.vision.opencv;
+
+public enum ContourIntersectionDirection {
+    None,
+    Up,
+    Down,
+    Left,
+    Right
+}

photon-server/src/main/java/org/photonvision/common/vision/opencv/ContourShape.java

@@ -0,0 +1,29 @@
+package org.photonvision.common.vision.opencv;
+
+import java.util.EnumSet;
+import java.util.HashMap;
+
+public enum ContourShape {
+    Custom(-1),
+    Circle(0),
+    Triangle(3),
+    Quadrilateral(4);
+
+    public final int sides;
+
+    ContourShape(int sides) {
+        this.sides = sides;
+    }
+
+    private static final HashMap<Integer, ContourShape> sidesToValueMap = new HashMap<>();
+
+    static {
+        for (var value : EnumSet.allOf(ContourShape.class)) {
+            sidesToValueMap.put(value.sides, value);
+        }
+    }
+
+    public static ContourShape fromSides(int sides) {
+        return sidesToValueMap.get(sides);
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/opencv/ContourSortMode.java

@@ -0,0 +1,29 @@
+package org.photonvision.common.vision.opencv;
+
+import org.photonvision.common.vision.target.PotentialTarget;
+import java.util.Comparator;
+import org.apache.commons.math3.util.FastMath;
+
+public enum ContourSortMode {
+    Largest(Comparator.comparingDouble(PotentialTarget::getArea)),
+    Smallest(Largest.getComparator().reversed()),
+    Highest(Comparator.comparingDouble(rect -> rect.getMinAreaRect().center.y)),
+    Lowest(Highest.getComparator().reversed()),
+    Leftmost(Comparator.comparingDouble(target -> target.getMinAreaRect().center.x)),
+    Rightmost(Leftmost.getComparator().reversed()),
+    Centermost(
+            Comparator.comparingDouble(
+                    rect ->
+                            (FastMath.pow(rect.getMinAreaRect().center.y, 2)
+                                    + FastMath.pow(rect.getMinAreaRect().center.x, 2))));
+
+    private Comparator<PotentialTarget> m_comparator;
+
+    ContourSortMode(Comparator<PotentialTarget> comparator) {
+        m_comparator = comparator;
+    }
+
+    public Comparator<PotentialTarget> getComparator() {
+        return m_comparator;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/opencv/DualMat.java

@@ -0,0 +1,8 @@
+package org.photonvision.common.vision.opencv;
+
+import org.opencv.core.Mat;
+
+public class DualMat {
+    public Mat first;
+    public Mat second;
+}

photon-server/src/main/java/org/photonvision/common/vision/opencv/Releasable.java

@@ -0,0 +1,5 @@
+package org.photonvision.common.vision.opencv;
+
+public interface Releasable {
+    void release();
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/CVPipe.java

@@ -0,0 +1,41 @@
+package org.photonvision.common.vision.pipe;
+
+import java.util.function.Function;
+
+/**
+* Defines a pipe. A pipe is a single step in a pipeline. This class is to be extended, never used
+* on its own.
+*
+* @param <I> Input type for the pipe
+* @param <O> Output type for the pipe
+* @param <P> Parameters type for the pipe
+*/
+public abstract class CVPipe<I, O, P> implements Function<I, CVPipeResult<O>> {
+
+    protected CVPipeResult<O> result = new CVPipeResult<>();
+    protected P params;
+
+    public void setParams(P params) {
+        this.params = params;
+    }
+
+    /**
+    * Runs the process for the pipe.
+    *
+    * @param in Input for pipe processing.
+    * @return Result of processing.
+    */
+    protected abstract O process(I in);
+
+    /**
+    * @param in Input for pipe processing.
+    * @return Result of processing.
+    */
+    @Override
+    public CVPipeResult<O> apply(I in) {
+        long pipeStartNanos = System.nanoTime();
+        result.result = process(in);
+        result.nanosElapsed = System.nanoTime() - pipeStartNanos;
+        return result;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/CVPipeResult.java

@@ -0,0 +1,6 @@
+package org.photonvision.common.vision.pipe;
+
+public class CVPipeResult<O> {
+    public O result;
+    public long nanosElapsed;
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/ImageFlipMode.java

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

photon-server/src/main/java/org/photonvision/common/vision/pipe/ImageRotationMode.java

@@ -0,0 +1,18 @@
+package org.photonvision.common.vision.pipe;
+
+public enum ImageRotationMode {
+    DEG_0(-1),
+    DEG_90(0),
+    DEG_180(1),
+    DEG_270(2);
+
+    public final int value;
+
+    ImageRotationMode(int value) {
+        this.value = value;
+    }
+
+    public boolean isRotated() {
+        return this.value == DEG_90.value || this.value == DEG_270.value;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/BlurPipe.java

@@ -0,0 +1,47 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+/** Represents a pipeline that blurs the image. */
+public class BlurPipe extends CVPipe<Mat, Mat, BlurPipe.BlurParams> {
+    /**
+    * Processes thos pipe.
+    *
+    * @param in Input for pipe processing.
+    * @return The processed frame.
+    */
+    @Override
+    protected Mat process(Mat in) {
+        Imgproc.blur(in, in, params.getBlurSize());
+        return in;
+    }
+
+    public static class BlurParams {
+        // Default BlurImagePrams with zero blur.
+        public static BlurParams DEFAULT = new BlurParams(0);
+
+        // Member to store the blur size.
+        private int m_blurSize;
+
+        /**
+        * Constructs a new BlurImageParams.
+        *
+        * @param blurSize The blur size.
+        */
+        public BlurParams(int blurSize) {
+            m_blurSize = blurSize;
+        }
+
+        /**
+        * Returns the blur size.
+        *
+        * @return The blur size.
+        */
+        public Size getBlurSize() {
+            return new Size(m_blurSize, m_blurSize);
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/Collect2dTargetsPipe.java

@@ -0,0 +1,99 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.util.numbers.DoubleCouple;
+import org.photonvision.common.vision.frame.FrameStaticProperties;
+import org.photonvision.common.vision.pipe.CVPipe;
+import com.chameleonvision.common.vision.target.*;
+import java.util.ArrayList;
+import java.util.List;
+import org.opencv.core.Point;
+import org.photonvision.common.vision.target.*;
+
+/** Represents a pipe that collects available 2d targets. */
+public class Collect2dTargetsPipe
+        extends CVPipe<
+                        List<PotentialTarget>, List<TrackedTarget>, Collect2dTargetsPipe.Collect2dTargetsParams> {
+
+    /**
+    * Processes this pipeline.
+    *
+    * @param in Input for pipe processing.
+    * @return A list of tracked targets.
+    */
+    @Override
+    protected List<TrackedTarget> process(List<PotentialTarget> in) {
+        List<TrackedTarget> targets = new ArrayList<>();
+
+        var calculationParams =
+                new TrackedTarget.TargetCalculationParameters(
+                        params.getOrientation() == TargetOrientation.Landscape,
+                        params.getOffsetPointRegion(),
+                        params.getUserOffsetPoint(),
+                        params.getFrameStaticProperties().centerPoint,
+                        new DoubleCouple(params.getCalibrationB(), params.getCalibrationM()),
+                        params.getOffsetMode(),
+                        params.getFrameStaticProperties().horizontalFocalLength,
+                        params.getFrameStaticProperties().verticalFocalLength,
+                        params.getFrameStaticProperties().imageArea);
+
+        for (PotentialTarget target : in) {
+            targets.add(new TrackedTarget(target, calculationParams));
+        }
+
+        return targets;
+    }
+
+    public static class Collect2dTargetsParams {
+        private FrameStaticProperties m_captureStaticProperties;
+        private RobotOffsetPointMode m_offsetMode;
+        private double m_calibrationM, m_calibrationB;
+        private Point m_userOffsetPoint;
+        private TargetOffsetPointEdge m_region;
+        private TargetOrientation m_orientation;
+
+        public Collect2dTargetsParams(
+                FrameStaticProperties captureStaticProperties,
+                RobotOffsetPointMode offsetMode,
+                double calibrationM,
+                double calibrationB,
+                Point calibrationPoint,
+                TargetOffsetPointEdge region,
+                TargetOrientation orientation) {
+            m_captureStaticProperties = captureStaticProperties;
+            m_offsetMode = offsetMode;
+            m_calibrationM = calibrationM;
+            m_calibrationB = calibrationB;
+            m_userOffsetPoint = calibrationPoint;
+            m_region = region;
+            m_orientation = orientation;
+        }
+
+        public FrameStaticProperties getFrameStaticProperties() {
+            return m_captureStaticProperties;
+        }
+
+        public RobotOffsetPointMode getOffsetMode() {
+            return m_offsetMode;
+        }
+
+        public double getCalibrationM() {
+            return m_calibrationM;
+        }
+
+        public double getCalibrationB() {
+            return m_calibrationB;
+        }
+
+        public Point getUserOffsetPoint() {
+            return m_userOffsetPoint;
+        }
+
+        public TargetOffsetPointEdge getOffsetPointRegion() {
+            return m_region;
+        }
+
+        public TargetOrientation getOrientation() {
+            return m_orientation;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/CornerDetectionPipe.java

@@ -0,0 +1,213 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.photonvision.common.vision.target.TrackedTarget;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Comparator;
+import java.util.List;
+import org.apache.commons.math3.util.FastMath;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
+import org.opencv.imgproc.Imgproc;
+
+public class CornerDetectionPipe
+        extends CVPipe<
+                        List<TrackedTarget>,
+                        List<TrackedTarget>,
+                        CornerDetectionPipe.CornerDetectionPipeParameters> {
+
+    Comparator<Point> leftRightComparator = Comparator.comparingDouble(point -> point.x);
+    Comparator<Point> verticalComparator = Comparator.comparingDouble(point -> point.y);
+    MatOfPoint2f polyOutput = new MatOfPoint2f();
+
+    @Override
+    protected List<TrackedTarget> process(List<TrackedTarget> targetList) {
+        for (var target : targetList) {
+            // detect corners. Might implement more algorithms later but
+            // APPROX_POLY_DP_AND_EXTREME_CORNERS should be year agnostic
+            switch (params.cornerDetectionStrategy) {
+                case APPROX_POLY_DP_AND_EXTREME_CORNERS:
+                    {
+                        var targetCorners =
+                                detectExtremeCornersByApproxPolyDp(target, params.calculateConvexHulls);
+                        target.setCorners(targetCorners);
+                        break;
+                    }
+                default:
+                    {
+                        break;
+                    }
+            }
+        }
+        return targetList;
+    }
+
+    /**
+    * @param target the target to find the corners of.
+    * @return the corners. left top, left bottom, right bottom, right top
+    */
+    private List<Point> findBoundingBoxCorners(TrackedTarget target) {
+        // extract the corners
+        var points = new Point[4];
+        target.m_mainContour.getMinAreaRect().points(points);
+
+        // find the tl/tr/bl/br corners
+        // first, min by left/right
+        var list_ = Arrays.asList(points);
+        list_.sort(leftRightComparator);
+        // of this, we now have left and right
+        // sort to get top and bottom
+        var left = new ArrayList<>(List.of(list_.get(0), list_.get(1)));
+        left.sort(verticalComparator);
+        var right = new ArrayList<>(List.of(list_.get(2), list_.get(3)));
+        right.sort(verticalComparator);
+
+        // tl tr bl br
+        var tl = left.get(0);
+        var bl = left.get(1);
+        var tr = right.get(0);
+        var br = right.get(1);
+
+        return List.of(tl, bl, br, tr);
+    }
+
+    /**
+    * @param a First point.
+    * @param b Second point.
+    * @return The straight line distance between them.
+    */
+    private static double distanceBetween(Point a, Point b) {
+        return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2));
+    }
+
+    /**
+    * @param a First point.
+    * @param b Second point.
+    * @return The straight line distance between them.
+    */
+    private static double distanceBetween(Translation2d a, Translation2d b) {
+        return FastMath.sqrt(
+                FastMath.pow(a.getX() - b.getX(), 2) + FastMath.pow(a.getY() - b.getY(), 2));
+    }
+
+    /**
+    * Find the 4 most extreme corners,
+    *
+    * @param target the target to track.
+    * @param convexHull weather to use the convex hull of the target.
+    * @return the 4 extreme corners of the contour.
+    */
+    private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) {
+        var centroid = target.getMinAreaRect().center;
+        Comparator<Point> distanceProvider =
+                Comparator.comparingDouble(
+                        (Point point) ->
+                                FastMath.sqrt(
+                                        FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
+
+        MatOfPoint2f targetContour;
+        if (convexHull) {
+            targetContour = target.m_mainContour.getConvexHull();
+        } else {
+            targetContour = target.m_mainContour.getMat2f();
+        }
+
+        /*
+        approximating a shape around the contours
+        Can be tuned to allow/disallow hulls
+        we want a number between 0 and 0.16 out of a percentage from 0 to 100
+        so take accuracy and divide by 600
+
+        Furthermore, we know that the contour is open if we haven't done convex hulls
+        and it has subcontours.
+        */
+        var isOpen = !convexHull && target.hasSubContours();
+        var peri = Imgproc.arcLength(targetContour, true);
+        Imgproc.approxPolyDP(
+                targetContour, polyOutput, params.accuracyPercentage / 600.0 * peri, !isOpen);
+
+        // we must have at least 4 corners for this strategy to work.
+        // If we are looking for an exact side count that is handled here too.
+        var pointList = new ArrayList<>(polyOutput.toList());
+        if (pointList.size() < 4 || (params.exactSideCount && params.sideCount != pointList.size()))
+            return null;
+
+        target.setApproximateBoundingPolygon(polyOutput);
+
+        // left top, left bottom, right bottom, right top
+        var boundingBoxCorners = findBoundingBoxCorners(target);
+
+        var distanceToTlComparator =
+                Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)));
+
+        var distanceToTrComparator =
+                Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)));
+
+        // top left and top right are the poly corners closest to the bouding box tl and tr
+        pointList.sort(distanceToTlComparator);
+        var tl = pointList.get(0);
+        pointList.remove(tl);
+        pointList.sort(distanceToTrComparator);
+        var tr = pointList.get(0);
+        pointList.remove(tr);
+
+        // at this point we look for points on the left/right of the center of the remaining points
+        // and maximize their distance from the center of the min area rectangle
+        var leftList = new ArrayList<Point>();
+        var rightList = new ArrayList<Point>();
+        var averageXCoordinate = 0;
+        for (var p : pointList) {
+            averageXCoordinate += p.x;
+        }
+        averageXCoordinate /= pointList.size();
+
+        // add points that are below the center of the min area rectangle of the target
+        for (var p : pointList) {
+            if (p.y
+                    > target.m_mainContour.getBoundingRect().y
+                            + target.m_mainContour.getBoundingRect().height / 2.0)
+                if (p.x < averageXCoordinate) {
+                    leftList.add(p);
+                } else {
+                    rightList.add(p);
+                }
+        }
+        if (leftList.isEmpty() || rightList.isEmpty()) return null;
+        leftList.sort(distanceProvider);
+        rightList.sort(distanceProvider);
+        var bl = leftList.get(leftList.size() - 1);
+        var br = rightList.get(rightList.size() - 1);
+        System.out.printf("Found points: TL (%s) BL (%s) BR (%s) TR (%s)\n", tl, bl, br, tr);
+        return List.of(tl, bl, br, tr);
+    }
+
+    public static class CornerDetectionPipeParameters {
+        private final DetectionStrategy cornerDetectionStrategy;
+
+        private final boolean calculateConvexHulls;
+        private final boolean exactSideCount;
+        private final int sideCount;
+
+        /** This number can be changed to change how "accurate" our approximate polygon must be. */
+        private final double accuracyPercentage;
+
+        public CornerDetectionPipeParameters(
+                DetectionStrategy cornerDetectionStrategy,
+                boolean calculateConvexHulls,
+                boolean exactSideCount,
+                int sideCount,
+                double accuracyPercentage) {
+            this.cornerDetectionStrategy = cornerDetectionStrategy;
+            this.calculateConvexHulls = calculateConvexHulls;
+            this.exactSideCount = exactSideCount;
+            this.sideCount = sideCount;
+            this.accuracyPercentage = accuracyPercentage;
+        }
+    }
+
+    public enum DetectionStrategy {
+        APPROX_POLY_DP_AND_EXTREME_CORNERS
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/Draw2dContoursPipe.java

@@ -0,0 +1,102 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.util.ColorHelper;
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.photonvision.common.vision.target.TrackedTarget;
+import java.awt.Color;
+import java.util.ArrayList;
+import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.*;
+import org.opencv.imgproc.Imgproc;
+
+public class Draw2dContoursPipe
+        extends CVPipe<Pair<Mat, List<TrackedTarget>>, Mat, Draw2dContoursPipe.Draw2dContoursParams> {
+
+    private List<MatOfPoint> m_drawnContours = new ArrayList<>();
+
+    @Override
+    protected Mat process(Pair<Mat, List<TrackedTarget>> in) {
+        if (!in.getRight().isEmpty()
+                && (params.showCentroid
+                        || params.showMaximumBox
+                        || params.showRotatedBox
+                        || params.showShape)) {
+
+            var centroidColour = ColorHelper.colorToScalar(params.centroidColor);
+            var maximumBoxColour = ColorHelper.colorToScalar(params.maximumBoxColor);
+            var rotatedBoxColour = ColorHelper.colorToScalar(params.rotatedBoxColor);
+            var shapeColour = ColorHelper.colorToScalar(params.shapeOutlineColour);
+
+            for (int i = 0; i < (params.showMultiple ? in.getRight().size() : 1); i++) {
+                Point[] vertices = new Point[4];
+                MatOfPoint contour = new MatOfPoint();
+
+                if (i != 0 && !params.showMultiple) {
+                    break;
+                }
+
+                TrackedTarget target = in.getRight().get(i);
+                RotatedRect r = target.getMinAreaRect();
+
+                if (r == null) continue;
+
+                m_drawnContours.forEach(Mat::release);
+                m_drawnContours.clear();
+                m_drawnContours = new ArrayList<>();
+
+                r.points(vertices);
+                contour.fromArray(vertices);
+                m_drawnContours.add(contour);
+
+                if (params.showRotatedBox) {
+                    Imgproc.drawContours(
+                            in.getLeft(), m_drawnContours, 0, rotatedBoxColour, params.boxOutlineSize);
+                }
+
+                if (params.showMaximumBox) {
+                    Rect box = Imgproc.boundingRect(contour);
+                    Imgproc.rectangle(
+                            in.getLeft(),
+                            new Point(box.x, box.y),
+                            new Point(box.x + box.width, box.y + box.height),
+                            maximumBoxColour,
+                            params.boxOutlineSize);
+                }
+
+                if (params.showShape) {
+                    Imgproc.drawContours(
+                            in.getLeft(),
+                            List.of(target.m_mainContour.mat),
+                            -1,
+                            shapeColour,
+                            params.boxOutlineSize);
+                }
+
+                if (params.showCentroid) {
+                    Imgproc.circle(in.getLeft(), target.getTargetOffsetPoint(), 3, centroidColour, 2);
+                }
+            }
+        }
+
+        return in.getLeft();
+    }
+
+    public static class Draw2dContoursParams {
+        public boolean showCentroid = true;
+        public boolean showMultiple = true;
+        public int boxOutlineSize = 1;
+        public boolean showRotatedBox = true;
+        public boolean showShape = false;
+        public boolean showMaximumBox = true;
+        public Color centroidColor = Color.GREEN;
+        public Color rotatedBoxColor = Color.BLUE;
+        public Color maximumBoxColor = Color.RED;
+        public Color shapeOutlineColour = Color.MAGENTA;
+
+        // TODO: set other params from UI/settings file?
+        public Draw2dContoursParams(boolean showMultipleTargets) {
+            this.showMultiple = showMultipleTargets;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/Draw2dCrosshairPipe.java

@@ -0,0 +1,62 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.util.ColorHelper;
+import org.photonvision.common.util.numbers.DoubleCouple;
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.photonvision.common.vision.target.RobotOffsetPointMode;
+import org.photonvision.common.vision.target.TrackedTarget;
+import java.awt.Color;
+import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.imgproc.Imgproc;
+
+public class Draw2dCrosshairPipe
+        extends CVPipe<Pair<Mat, List<TrackedTarget>>, Mat, Draw2dCrosshairPipe.Draw2dCrosshairParams> {
+
+    @Override
+    protected Mat process(Pair<Mat, List<TrackedTarget>> in) {
+        Mat image = in.getLeft();
+
+        if (params.m_showCrosshair) {
+            double x = image.cols() / 2.0;
+            double y = image.rows() / 2.0;
+            double scale = image.cols() / 32.0;
+
+            switch (params.m_calibrationMode) {
+                case Single:
+                    if (!params.m_calibrationPoint.isEmpty()) {
+                        x = params.m_calibrationPoint.getFirst();
+                        y = params.m_calibrationPoint.getSecond();
+                    }
+                    break;
+                case Dual:
+                    // TODO: draw crosshair based on dual calibration
+                    break;
+            }
+
+            Point xMax = new Point(x + scale, y);
+            Point xMin = new Point(x - scale, y);
+            Point yMax = new Point(x, y + scale);
+            Point yMin = new Point(x, y - scale);
+
+            Imgproc.line(image, xMax, xMin, ColorHelper.colorToScalar(params.m_crosshairColor));
+            Imgproc.line(image, yMax, yMin, ColorHelper.colorToScalar(params.m_crosshairColor));
+        }
+        return image;
+    }
+
+    public static class Draw2dCrosshairParams {
+        private RobotOffsetPointMode m_calibrationMode;
+        private DoubleCouple m_calibrationPoint;
+        public boolean m_showCrosshair = true;
+        public Color m_crosshairColor = Color.GREEN;
+
+        public Draw2dCrosshairParams(
+                RobotOffsetPointMode calibrationMode, DoubleCouple calibrationPoint) {
+            m_calibrationMode = calibrationMode;
+            m_calibrationPoint = calibrationPoint;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/Draw3dTargetsPipe.java

@@ -0,0 +1,122 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.calibration.CameraCalibrationCoefficients;
+import org.photonvision.common.util.ColorHelper;
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.photonvision.common.vision.target.TargetModel;
+import org.photonvision.common.vision.target.TrackedTarget;
+import java.awt.*;
+import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.imgproc.Imgproc;
+
+public class Draw3dTargetsPipe
+        extends CVPipe<Pair<Mat, List<TrackedTarget>>, Mat, Draw3dTargetsPipe.Draw3dContoursParams> {
+
+    @Override
+    protected Mat process(Pair<Mat, List<TrackedTarget>> in) {
+        for (var target : in.getRight()) {
+
+            // draw convex hull
+            var pointMat = new MatOfPoint();
+            target.m_mainContour.getConvexHull().convertTo(pointMat, CvType.CV_32S);
+            Imgproc.drawContours(
+                    in.getLeft(), List.of(pointMat), -1, ColorHelper.colorToScalar(Color.green), 1);
+
+            // draw approximate polygon
+            var poly = target.getApproximateBoundingPolygon();
+            if (poly != null) {
+                poly.convertTo(pointMat, CvType.CV_32S);
+                Imgproc.drawContours(
+                        in.getLeft(), List.of(pointMat), -1, ColorHelper.colorToScalar(Color.blue), 2);
+            }
+
+            // Draw floor and top
+            if (target.getCameraRelativeRvec() != null && target.getCameraRelativeTvec() != null) {
+                var tempMat = new MatOfPoint2f();
+                var jac = new Mat();
+                var bottomModel = params.targetModel.getVisualizationBoxBottom();
+                var topModel = params.targetModel.getVisualizationBoxTop();
+                Calib3d.projectPoints(
+                        bottomModel,
+                        target.getCameraRelativeRvec(),
+                        target.getCameraRelativeTvec(),
+                        params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(),
+                        params.cameraCalibrationCoefficients.getCameraExtrinsicsMat(),
+                        tempMat,
+                        jac);
+                var bottomPoints = tempMat.toList();
+                Calib3d.projectPoints(
+                        topModel,
+                        target.getCameraRelativeRvec(),
+                        target.getCameraRelativeTvec(),
+                        params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(),
+                        params.cameraCalibrationCoefficients.getCameraExtrinsicsMat(),
+                        tempMat,
+                        jac);
+                var topPoints = tempMat.toList();
+                // floor, then pillers, then top
+                for (int i = 0; i < bottomPoints.size(); i++) {
+                    Imgproc.line(
+                            in.getLeft(),
+                            bottomPoints.get(i),
+                            bottomPoints.get((i + 1) % (bottomPoints.size())),
+                            ColorHelper.colorToScalar(Color.green),
+                            3);
+                }
+                for (int i = 0; i < bottomPoints.size(); i++) {
+                    Imgproc.line(
+                            in.getLeft(),
+                            bottomPoints.get(i),
+                            topPoints.get(i),
+                            ColorHelper.colorToScalar(Color.blue),
+                            3);
+                }
+                for (int i = 0; i < topPoints.size(); i++) {
+                    Imgproc.line(
+                            in.getLeft(),
+                            topPoints.get(i),
+                            topPoints.get((i + 1) % (bottomPoints.size())),
+                            ColorHelper.colorToScalar(Color.orange),
+                            3);
+                }
+
+                jac.release();
+            }
+            pointMat.release();
+
+            // draw corners
+            var corners = target.getTargetCorners();
+            if (corners != null && !corners.isEmpty()) {
+                for (var corner : corners) {
+                    Imgproc.circle(
+                            in.getLeft(),
+                            corner,
+                            params.radius,
+                            ColorHelper.colorToScalar(params.color),
+                            params.radius);
+                }
+            }
+        }
+
+        return in.getLeft();
+    }
+
+    public static class Draw3dContoursParams {
+        private final int radius = 2;
+        private final Color color = Color.RED;
+        private final TargetModel targetModel;
+        private final CameraCalibrationCoefficients cameraCalibrationCoefficients;
+
+        public Draw3dContoursParams(
+                CameraCalibrationCoefficients cameraCalibrationCoefficients, TargetModel targetModel) {
+            this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
+            this.targetModel = targetModel;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/DrawCornerDetectionPipe.java

@@ -0,0 +1,32 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.photonvision.common.vision.target.TrackedTarget;
+import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+
+public class DrawCornerDetectionPipe
+        extends CVPipe<Pair<Mat, List<TrackedTarget>>, Mat, DrawCornerDetectionPipe.DrawCornerParams> {
+
+    @Override
+    protected Mat process(Pair<Mat, List<TrackedTarget>> in) {
+        Mat image = in.getLeft();
+
+        for (var target : in.getRight()) {
+            var corners = target.getTargetCorners();
+            for (var corner : corners) {
+                Imgproc.circle(image, corner, params.dotRadius, params.dotColor);
+            }
+        }
+
+        return image;
+    }
+
+    public static class DrawCornerParams {
+        int dotRadius;
+        Scalar dotColor;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/ErodeDilatePipe.java

@@ -0,0 +1,44 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+public class ErodeDilatePipe extends CVPipe<Mat, Mat, ErodeDilatePipe.ErodeDilateParams> {
+    @Override
+    protected Mat process(Mat in) {
+        if (params.shouldErode()) {
+            Imgproc.erode(in, in, params.getKernel());
+        }
+        if (params.shouldDilate()) {
+            Imgproc.dilate(in, in, params.getKernel());
+        }
+        return in;
+    }
+
+    public static class ErodeDilateParams {
+        private boolean m_erode;
+        private boolean m_dilate;
+        private Mat m_kernel;
+
+        public ErodeDilateParams(boolean erode, boolean dilate, int kernelSize) {
+            m_erode = erode;
+            m_dilate = dilate;
+            m_kernel =
+                    Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(kernelSize, kernelSize));
+        }
+
+        public boolean shouldErode() {
+            return m_erode;
+        }
+
+        public boolean shouldDilate() {
+            return m_dilate;
+        }
+
+        public Mat getKernel() {
+            return m_kernel;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FilterContoursPipe.java

@@ -0,0 +1,88 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.common.util.numbers.DoubleCouple;
+import org.photonvision.common.vision.frame.FrameStaticProperties;
+import org.photonvision.common.vision.opencv.Contour;
+import org.photonvision.common.vision.pipe.CVPipe;
+import java.util.ArrayList;
+import java.util.List;
+import org.opencv.core.Rect;
+import org.opencv.core.RotatedRect;
+
+public class FilterContoursPipe
+        extends CVPipe<List<Contour>, List<Contour>, FilterContoursPipe.FilterContoursParams> {
+
+    List<Contour> m_filteredContours = new ArrayList<>();
+
+    @Override
+    protected List<Contour> process(List<Contour> in) {
+        m_filteredContours.clear();
+        for (Contour contour : in) {
+            try {
+                filterContour(contour);
+            } catch (Exception e) {
+                System.err.println("An error occurred while filtering contours.");
+                e.printStackTrace();
+            }
+        }
+        return m_filteredContours;
+    }
+
+    private void filterContour(Contour contour) {
+        // Area Filtering.
+        double contourArea = contour.getArea();
+        double areaRatio = (contourArea / params.getCamProperties().imageArea);
+        double minArea = MathUtils.sigmoid(params.getArea().getFirst());
+        double maxArea = MathUtils.sigmoid(params.getArea().getSecond());
+        if (areaRatio < minArea || areaRatio > maxArea) return;
+
+        // Extent Filtering.
+        RotatedRect minAreaRect = contour.getMinAreaRect();
+        double minExtent = params.getExtent().getFirst() * minAreaRect.size.area() / 100;
+        double maxExtent = params.getExtent().getSecond() * minAreaRect.size.area() / 100;
+        if (contourArea <= minExtent || contourArea >= maxExtent) return;
+
+        // Aspect Ratio Filtering.
+        Rect boundingRect = contour.getBoundingRect();
+        double aspectRatio = (double) boundingRect.width / boundingRect.height;
+        if (aspectRatio < params.getRatio().getFirst() || aspectRatio > params.getRatio().getSecond())
+            return;
+
+        m_filteredContours.add(contour);
+    }
+
+    public static class FilterContoursParams {
+        private DoubleCouple m_area;
+        private DoubleCouple m_ratio;
+        private DoubleCouple m_extent;
+        private FrameStaticProperties m_camProperties;
+
+        public FilterContoursParams(
+                DoubleCouple area,
+                DoubleCouple ratio,
+                DoubleCouple extent,
+                FrameStaticProperties camProperties) {
+            this.m_area = area;
+            this.m_ratio = ratio;
+            this.m_extent = extent;
+            this.m_camProperties = camProperties;
+        }
+
+        public DoubleCouple getArea() {
+            return m_area;
+        }
+
+        public DoubleCouple getRatio() {
+            return m_ratio;
+        }
+
+        public DoubleCouple getExtent() {
+            return m_extent;
+        }
+
+        public FrameStaticProperties getCamProperties() {
+            return m_camProperties;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FilterShapesPipe.java

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

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FindCirclesPipe.java

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

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FindContoursPipe.java

@@ -0,0 +1,31 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.opencv.Contour;
+import org.photonvision.common.vision.pipe.CVPipe;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.stream.Collectors;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.imgproc.Imgproc;
+
+public class FindContoursPipe
+        extends CVPipe<Mat, List<Contour>, FindContoursPipe.FindContoursParams> {
+
+    private List<MatOfPoint> m_foundContours = new ArrayList<>();
+
+    @Override
+    protected List<Contour> process(Mat in) {
+        for (var m : m_foundContours) {
+            m.release(); // necessary?
+        }
+        m_foundContours.clear();
+
+        Imgproc.findContours(
+                in, m_foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1);
+
+        return m_foundContours.stream().map(Contour::new).collect(Collectors.toList());
+    }
+
+    public static class FindContoursParams {}
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FindPolygonPipe.java

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

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FindShapesPipe.java

@@ -0,0 +1,41 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.opencv.CVShape;
+import org.photonvision.common.vision.opencv.Contour;
+import org.photonvision.common.vision.opencv.ContourShape;
+import org.photonvision.common.vision.pipe.CVPipe;
+import java.util.List;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.imgproc.Imgproc;
+
+public class FindShapesPipe
+        extends CVPipe<List<Contour>, List<CVShape>, FindShapesPipe.FindShapesParams> {
+
+    MatOfPoint2f approxCurve = new MatOfPoint2f();
+
+    @Override
+    protected List<CVShape> process(List<Contour> in) {
+        approxCurve.release();
+        approxCurve = new MatOfPoint2f();
+
+        for (var contour : in) {
+
+            if (params.desiredShape == ContourShape.Circle) {
+
+            } else {
+                int desiredSides = params.desiredShape.sides;
+                Imgproc.approxPolyDP(contour.getMat2f(), approxCurve, params.approxEpsilon, true);
+
+                //                int actualSides = approxCurve.
+                //                switch ()
+                System.out.println("fugg");
+            }
+        }
+        return List.of();
+    }
+
+    public static class FindShapesParams {
+        double approxEpsilon = 0.05;
+        ContourShape desiredShape;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/GroupContoursPipe.java

@@ -0,0 +1,85 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.opencv.Contour;
+import org.photonvision.common.vision.opencv.ContourGroupingMode;
+import org.photonvision.common.vision.opencv.ContourIntersectionDirection;
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.photonvision.common.vision.target.PotentialTarget;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+public class GroupContoursPipe
+        extends CVPipe<List<Contour>, List<PotentialTarget>, GroupContoursPipe.GroupContoursParams> {
+
+    private List<PotentialTarget> m_targets = new ArrayList<>();
+
+    @Override
+    protected List<PotentialTarget> process(List<Contour> input) {
+        for (var target : m_targets) {
+            target.release();
+        }
+
+        m_targets.clear();
+
+        if (params.getGroup() == ContourGroupingMode.Single) {
+            for (var contour : input) {
+                m_targets.add(new PotentialTarget(contour));
+            }
+        } else {
+            int groupingCount = params.getGroup().count;
+
+            if (input.size() > groupingCount) {
+                input.sort(Contour.SortByMomentsX);
+                // also why reverse? shouldn't the sort comparator just get reversed?
+                Collections.reverse(input);
+                // find out next time on Code Mysteries...
+
+                for (int i = 0; i < input.size() - 1; i++) {
+                    // make a list of the desired count of contours to group
+                    List<Contour> groupingSet;
+                    try {
+                        groupingSet = input.subList(i, i + groupingCount);
+                    } catch (IndexOutOfBoundsException e) {
+                        continue;
+                    }
+                    try {
+
+                        // FYI: This method only takes 2 contours!
+                        Contour groupedContour =
+                                Contour.groupContoursByIntersection(
+                                        groupingSet.get(0), groupingSet.get(1), params.getIntersection());
+
+                        if (groupedContour != null) {
+                            m_targets.add(new PotentialTarget(groupedContour, groupingSet));
+                            i += (groupingCount - 1);
+                        }
+                    } catch (Exception ex) {
+                        ex.printStackTrace();
+                    }
+                }
+            }
+        }
+        return m_targets;
+    }
+
+    public static class GroupContoursParams {
+        private ContourGroupingMode m_group;
+        private ContourIntersectionDirection m_intersection;
+
+        public GroupContoursParams(
+                ContourGroupingMode group, ContourIntersectionDirection intersectionDirection) {
+            m_group = group;
+            m_intersection = intersectionDirection;
+        }
+
+        public ContourGroupingMode getGroup() {
+            return m_group;
+        }
+
+        public ContourIntersectionDirection getIntersection() {
+            return m_intersection;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/HSVPipe.java

@@ -0,0 +1,50 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.util.numbers.IntegerCouple;
+import org.photonvision.common.vision.pipe.CVPipe;
+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 extends CVPipe<Mat, Mat, HSVPipe.HSVParams> {
+
+    private Mat m_outputMat = new Mat();
+
+    @Override
+    protected Mat process(Mat in) {
+        in.copyTo(m_outputMat);
+        try {
+            Imgproc.cvtColor(m_outputMat, m_outputMat, Imgproc.COLOR_BGR2HSV, 3);
+            Core.inRange(m_outputMat, params.getHsvLower(), params.getHsvUpper(), m_outputMat);
+        } catch (CvException e) {
+            System.err.println("(HSVPipe) Exception thrown by OpenCV: \n" + e.getMessage());
+        }
+
+        return m_outputMat;
+    }
+
+    public static class HSVParams {
+        private Scalar m_hsvLower;
+        private Scalar m_hsvUpper;
+
+        public HSVParams(IntegerCouple hue, IntegerCouple saturation, IntegerCouple value) {
+            m_hsvLower = new Scalar(hue.getFirst(), saturation.getFirst(), value.getFirst());
+            m_hsvUpper = new Scalar(hue.getSecond(), saturation.getSecond(), value.getSecond());
+        }
+
+        public HSVParams(Scalar hsvLower, Scalar hsvUpper) {
+            m_hsvLower = hsvLower;
+            m_hsvUpper = hsvUpper;
+        }
+
+        public Scalar getHsvLower() {
+            return m_hsvLower;
+        }
+
+        public Scalar getHsvUpper() {
+            return m_hsvUpper;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/OutputMatPipe.java

@@ -0,0 +1,43 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.opencv.DualMat;
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.opencv.core.CvException;
+import org.opencv.core.Mat;
+import org.opencv.imgproc.Imgproc;
+
+public class OutputMatPipe extends CVPipe<DualMat, Mat, OutputMatPipe.OutputMatParams> {
+
+    private Mat m_outputMat = new Mat();
+
+    @Override
+    protected Mat process(DualMat in) {
+        Mat rawCam = in.first;
+        Mat hsv = in.second;
+        if (params.showThreshold()) {
+            // convert input mat
+            try {
+                hsv.copyTo(m_outputMat);
+                Imgproc.cvtColor(m_outputMat, m_outputMat, Imgproc.COLOR_GRAY2BGR, 3);
+            } catch (CvException e) {
+                System.err.println("(OutputMatPipe) Exception thrown by OpenCV: \n" + e.getMessage());
+            }
+        } else {
+            m_outputMat = rawCam;
+        }
+
+        return m_outputMat;
+    }
+
+    public static class OutputMatParams {
+        private boolean m_showThreshold;
+
+        public OutputMatParams(boolean showThreshold) {
+            m_showThreshold = showThreshold;
+        }
+
+        public boolean showThreshold() {
+            return m_showThreshold;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/ResizeImagePipe.java

@@ -0,0 +1,70 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.frame.FrameDivisor;
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+/** Pipe that resizes an image to a given resolution */
+public class ResizeImagePipe extends CVPipe<Mat, Mat, ResizeImagePipe.ResizeImageParams> {
+
+    public ResizeImagePipe() {
+        setParams(ResizeImageParams.DEFAULT);
+    }
+
+    public ResizeImagePipe(ResizeImageParams params) {
+        setParams(params);
+    }
+
+    /**
+    * Process this pipe
+    *
+    * @param in {@link Mat} to be resized
+    * @return Resized {@link Mat}
+    */
+    @Override
+    protected Mat process(Mat in) {
+
+        // if a divisor is set, use that instead of a size.
+        if (params.getDivisor() != null) {
+            int width = in.cols() / params.getDivisor().value;
+            int height = in.rows() / params.getDivisor().value;
+            setParams(new ResizeImageParams(width, height));
+        }
+
+        Imgproc.resize(in, in, params.getSize());
+        return in;
+    }
+
+    public static class ResizeImageParams {
+        public static ResizeImageParams DEFAULT = new ResizeImageParams(320, 240);
+
+        private Size size;
+        private int width;
+        private int height;
+        private FrameDivisor divisor;
+
+        public ResizeImageParams() {
+            this(DEFAULT.width, DEFAULT.height);
+        }
+
+        public ResizeImageParams(int width, int height) {
+            this.width = width;
+            this.height = height;
+            size = new Size(new double[] {width, height});
+        }
+
+        public ResizeImageParams(FrameDivisor divisor) {
+            this.divisor = divisor;
+        }
+
+        public Size getSize() {
+            return size;
+        }
+
+        public FrameDivisor getDivisor() {
+            return divisor;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/RotateImagePipe.java

@@ -0,0 +1,44 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.photonvision.common.vision.pipe.ImageRotationMode;
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+
+/** Pipe that rotates an image to a given orientation */
+public class RotateImagePipe extends CVPipe<Mat, Mat, RotateImagePipe.RotateImageParams> {
+
+    public RotateImagePipe() {
+        setParams(RotateImageParams.DEFAULT);
+    }
+
+    public RotateImagePipe(RotateImageParams params) {
+        setParams(params);
+    }
+
+    /**
+    * Process this pipe
+    *
+    * @param in {@link Mat} to be rotated
+    * @return Rotated {@link Mat}
+    */
+    @Override
+    protected Mat process(Mat in) {
+        Core.rotate(in, in, params.rotation.value);
+        return in;
+    }
+
+    public static class RotateImageParams {
+        public static RotateImageParams DEFAULT = new RotateImageParams(ImageRotationMode.DEG_0);
+
+        public ImageRotationMode rotation;
+
+        public RotateImageParams() {
+            rotation = DEFAULT.rotation;
+        }
+
+        public RotateImageParams(ImageRotationMode rotation) {
+            this.rotation = rotation;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/SolvePNPPipe.java

@@ -0,0 +1,139 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.calibration.CameraCalibrationCoefficients;
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.photonvision.common.vision.target.TargetModel;
+import org.photonvision.common.vision.target.TrackedTarget;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import java.util.List;
+import org.apache.commons.math3.util.FastMath;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Scalar;
+
+public class SolvePNPPipe
+        extends CVPipe<List<TrackedTarget>, List<TrackedTarget>, SolvePNPPipe.SolvePNPPipeParams> {
+
+    private MatOfPoint2f imagePoints = new MatOfPoint2f();
+
+    @Override
+    protected List<TrackedTarget> process(List<TrackedTarget> targetList) {
+        for (var target : targetList) {
+            calculateTargetPose(target);
+        }
+        return targetList;
+    }
+
+    private void calculateTargetPose(TrackedTarget target) {
+        Pose2d targetPose;
+
+        var corners = target.getTargetCorners();
+        if (corners == null
+                || corners.isEmpty()
+                || params.cameraCoefficients.getCameraIntrinsicsMat() == null
+                || params.cameraCoefficients.getCameraExtrinsicsMat() == null) {
+            targetPose = new Pose2d();
+            return;
+        }
+        this.imagePoints.fromList(corners);
+
+        var rVec = new Mat();
+        var tVec = new Mat();
+        try {
+            Calib3d.solvePnP(
+                    params.targetModel.getRealWorldTargetCoordinates(),
+                    imagePoints,
+                    params.cameraCoefficients.getCameraIntrinsicsMat(),
+                    params.cameraCoefficients.getCameraExtrinsicsMat(),
+                    rVec,
+                    tVec);
+        } catch (Exception e) {
+            e.printStackTrace();
+            return;
+        }
+
+        target.setCameraRelativeTvec(tVec);
+        target.setCameraRelativeRvec(rVec);
+
+        targetPose = correctLocationForCameraPitch(tVec, rVec, params.cameraPitchAngle);
+
+        target.setRobotRelativePose(targetPose);
+    }
+
+    Mat rotationMatrix = new Mat();
+    Mat inverseRotationMatrix = new Mat();
+    Mat pzeroWorld = new Mat();
+    Mat kMat = new Mat();
+    Mat scaledTvec;
+
+    @SuppressWarnings("DuplicatedCode") // yes I know we have another solvePNP pipe
+    private Pose2d correctLocationForCameraPitch(Mat tVec, Mat rVec, Rotation2d cameraPitchAngle) {
+        // Algorithm from team 5190 Green Hope Falcons. Can also be found in Ligerbot's vision
+        // whitepaper
+        var tiltAngle = cameraPitchAngle.getRadians();
+
+        // the left/right distance to the target, unchanged by tilt.
+        var x = tVec.get(0, 0)[0];
+
+        // Z distance in the flat plane is given by
+        // Z_field = z cos theta + y sin theta.
+        // Z is the distance "out" of the camera (straight forward).
+        var zField =
+                tVec.get(2, 0)[0] * FastMath.cos(tiltAngle) + tVec.get(1, 0)[0] * FastMath.sin(tiltAngle);
+
+        Calib3d.Rodrigues(rVec, rotationMatrix);
+        Core.transpose(rotationMatrix, inverseRotationMatrix);
+
+        scaledTvec = matScale(tVec, -1);
+
+        Core.gemm(inverseRotationMatrix, scaledTvec, 1, kMat, 0, pzeroWorld);
+        scaledTvec.release();
+
+        var angle2 = FastMath.atan2(pzeroWorld.get(0, 0)[0], pzeroWorld.get(2, 0)[0]);
+
+        // target rotation is the rotation of the target relative to straight ahead. this number
+        // should be unchanged if the robot purely translated left/right.
+        var targetRotation = -angle2; // radians
+
+        // We want a vector that is X forward and Y left.
+        // We have a Z_field (out of the camera projected onto the field), and an X left/right.
+        // so Z_field becomes X, and X becomes Y
+
+        //noinspection SuspiciousNameCombination
+        var targetLocation = new Translation2d(zField, -x);
+        return new Pose2d(targetLocation, new Rotation2d(targetRotation));
+    }
+
+    /**
+    * Element-wise scale a matrix by a given factor
+    *
+    * @param src the source matrix
+    * @param factor by how much to scale each element
+    * @return the scaled matrix
+    */
+    private static Mat matScale(Mat src, double factor) {
+        Mat dst = new Mat(src.rows(), src.cols(), src.type());
+        Scalar s = new Scalar(factor);
+        Core.multiply(src, s, dst);
+        return dst;
+    }
+
+    public static class SolvePNPPipeParams {
+        private final CameraCalibrationCoefficients cameraCoefficients;
+        private final Rotation2d cameraPitchAngle;
+        private final TargetModel targetModel;
+
+        public SolvePNPPipeParams(
+                CameraCalibrationCoefficients cameraCoefficients,
+                Rotation2d cameraPitchAngle,
+                TargetModel targetModel) {
+            this.cameraCoefficients = cameraCoefficients;
+            this.cameraPitchAngle = cameraPitchAngle;
+            this.targetModel = targetModel;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/SortContoursPipe.java

@@ -0,0 +1,64 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.frame.FrameStaticProperties;
+import org.photonvision.common.vision.opencv.ContourSortMode;
+import org.photonvision.common.vision.pipe.CVPipe;
+import org.photonvision.common.vision.target.PotentialTarget;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+import org.apache.commons.math3.util.FastMath;
+
+public class SortContoursPipe
+        extends CVPipe<
+                        List<PotentialTarget>, List<PotentialTarget>, SortContoursPipe.SortContoursParams> {
+
+    private List<PotentialTarget> m_sortedContours = new ArrayList<>();
+
+    @Override
+    protected List<PotentialTarget> process(List<PotentialTarget> in) {
+        m_sortedContours.clear();
+        if (in.size() > 0) {
+            m_sortedContours.addAll(in);
+            if (params.getSortMode() != ContourSortMode.Centermost) {
+                m_sortedContours.sort(params.getSortMode().getComparator());
+            } else {
+                m_sortedContours.sort(Comparator.comparingDouble(this::calcSquareCenterDistance));
+            }
+        }
+
+        return new ArrayList<>(
+                m_sortedContours.subList(0, Math.min(in.size(), params.getMaxTargets() - 1)));
+    }
+
+    private double calcSquareCenterDistance(PotentialTarget rect) {
+        return FastMath.sqrt(
+                FastMath.pow(params.getCamProperties().centerX - rect.getMinAreaRect().center.x, 2)
+                        + FastMath.pow(params.getCamProperties().centerY - rect.getMinAreaRect().center.y, 2));
+    }
+
+    public static class SortContoursParams {
+        private ContourSortMode m_sortMode;
+        private FrameStaticProperties m_camProperties;
+        private int m_maxTargets;
+
+        public SortContoursParams(
+                ContourSortMode sortMode, FrameStaticProperties camProperties, int maxTargets) {
+            m_sortMode = sortMode;
+            m_camProperties = camProperties;
+            m_maxTargets = maxTargets;
+        }
+
+        public ContourSortMode getSortMode() {
+            return m_sortMode;
+        }
+
+        public FrameStaticProperties getCamProperties() {
+            return m_camProperties;
+        }
+
+        public int getMaxTargets() {
+            return m_maxTargets;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/SpeckleRejectPipe.java

@@ -0,0 +1,50 @@
+package org.photonvision.common.vision.pipe.impl;
+
+import org.photonvision.common.vision.opencv.Contour;
+import org.photonvision.common.vision.pipe.CVPipe;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class SpeckleRejectPipe
+        extends CVPipe<List<Contour>, List<Contour>, SpeckleRejectPipe.SpeckleRejectParams> {
+
+    private List<Contour> m_despeckledContours = new ArrayList<>();
+
+    @Override
+    protected List<Contour> process(List<Contour> in) {
+        for (var c : m_despeckledContours) {
+            c.mat.release();
+        }
+        m_despeckledContours.clear();
+
+        if (in.size() > 0) {
+            double averageArea = 0.0;
+            for (Contour c : in) {
+                averageArea += c.getArea();
+            }
+            averageArea /= in.size();
+
+            double minAllowedArea = params.getMinPercentOfAvg() / 100.0 * averageArea;
+            for (Contour c : in) {
+                if (c.getArea() >= minAllowedArea) {
+                    m_despeckledContours.add(c);
+                }
+            }
+        }
+
+        return m_despeckledContours;
+    }
+
+    public static class SpeckleRejectParams {
+        private double m_minPercentOfAvg;
+
+        public SpeckleRejectParams(double minPercentOfAvg) {
+            m_minPercentOfAvg = minPercentOfAvg;
+        }
+
+        public double getMinPercentOfAvg() {
+            return m_minPercentOfAvg;
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipeline/AdvancedPipelineSettings.java

@@ -0,0 +1,104 @@
+package org.photonvision.common.vision.pipeline;
+
+import org.photonvision.common.util.numbers.DoubleCouple;
+import org.photonvision.common.util.numbers.IntegerCouple;
+import org.photonvision.common.vision.opencv.ContourSortMode;
+import org.photonvision.common.vision.target.RobotOffsetPointMode;
+import org.photonvision.common.vision.target.TargetOffsetPointEdge;
+import org.photonvision.common.vision.target.TargetOrientation;
+
+import java.util.Objects;
+
+public class AdvancedPipelineSettings extends CVPipelineSettings {
+
+    public AdvancedPipelineSettings() {
+        ledMode = true;
+    }
+
+    public IntegerCouple hsvHue = new IntegerCouple(50, 180);
+    public IntegerCouple hsvSaturation = new IntegerCouple(50, 255);
+    public IntegerCouple hsvValue = new IntegerCouple(50, 255);
+
+    public boolean outputShowThresholded = false;
+    public boolean outputShowMultipleTargets = false;
+
+    public boolean erode = false;
+    public boolean dilate = false;
+
+    public DoubleCouple contourArea = new DoubleCouple(0.0, 100.0);
+    public DoubleCouple contourRatio = new DoubleCouple(0.0, 20.0);
+    public DoubleCouple contourExtent = new DoubleCouple(0.0, 100.0);
+    public int contourSpecklePercentage = 5;
+
+    // the order in which to sort contours to find the most desirable
+    public ContourSortMode contourSortMode = ContourSortMode.Largest;
+
+    // the edge (or not) of the target to consider the center point (Top, Bottom, Left, Right,
+    // Center)
+    public TargetOffsetPointEdge contourTargetOffsetPointEdge = TargetOffsetPointEdge.Center;
+
+    // orientation of the target in terms of aspect ratio
+    public TargetOrientation contourTargetOrientation = TargetOrientation.Landscape;
+
+    // the mode in which to offset target center point based on the camera being offset on the
+    // robot
+    // (None, Single Point, Dual Point)
+    public RobotOffsetPointMode offsetRobotOffsetMode = RobotOffsetPointMode.None;
+
+    // the point set by the user in Single Point Offset mode (maybe double too? idr)
+    public DoubleCouple offsetCalibrationPoint = new DoubleCouple();
+
+    // the two values that define the line of the Dual Point Offset calibration (think y=mx+b)
+    public double offsetDualLineM = 1;
+    public double offsetDualLineB = 0;
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        if (!super.equals(o)) return false;
+        AdvancedPipelineSettings that = (AdvancedPipelineSettings) o;
+        return outputShowThresholded == that.outputShowThresholded
+                && outputShowMultipleTargets == that.outputShowMultipleTargets
+                && erode == that.erode
+                && dilate == that.dilate
+                && contourSpecklePercentage == that.contourSpecklePercentage
+                && Double.compare(that.offsetDualLineM, offsetDualLineM) == 0
+                && Double.compare(that.offsetDualLineB, offsetDualLineB) == 0
+                && hsvHue.equals(that.hsvHue)
+                && hsvSaturation.equals(that.hsvSaturation)
+                && hsvValue.equals(that.hsvValue)
+                && contourArea.equals(that.contourArea)
+                && contourRatio.equals(that.contourRatio)
+                && contourExtent.equals(that.contourExtent)
+                && contourSortMode == that.contourSortMode
+                && contourTargetOffsetPointEdge == that.contourTargetOffsetPointEdge
+                && contourTargetOrientation == that.contourTargetOrientation
+                && offsetRobotOffsetMode == that.offsetRobotOffsetMode
+                && offsetCalibrationPoint.equals(that.offsetCalibrationPoint);
+    }
+
+    @Override
+    public int hashCode() {
+        return Objects.hash(
+                super.hashCode(),
+                hsvHue,
+                hsvSaturation,
+                hsvValue,
+                outputShowThresholded,
+                outputShowMultipleTargets,
+                erode,
+                dilate,
+                contourArea,
+                contourRatio,
+                contourExtent,
+                contourSpecklePercentage,
+                contourSortMode,
+                contourTargetOffsetPointEdge,
+                contourTargetOrientation,
+                offsetRobotOffsetMode,
+                offsetCalibrationPoint,
+                offsetDualLineM,
+                offsetDualLineB);
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipeline/CVPipeline.java

@@ -0,0 +1,32 @@
+package org.photonvision.common.vision.pipeline;
+
+import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.common.vision.frame.Frame;
+import org.photonvision.common.vision.frame.FrameStaticProperties;
+
+public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings> {
+    protected S settings;
+
+    protected abstract void setPipeParams(FrameStaticProperties frameStaticProperties, S settings);
+
+    protected abstract R process(Frame frame, S settings);
+
+    public S getSettings() {
+        return settings;
+    }
+
+    public R run(Frame frame) {
+        long pipelineStartNanos = System.nanoTime();
+
+        if (settings == null) {
+            throw new RuntimeException("No settings provided for pipeline!");
+        }
+        setPipeParams(frame.frameStaticProperties, settings);
+
+        R result = process(frame, settings);
+
+        result.setLatencyMillis(MathUtils.nanosToMillis(System.nanoTime() - pipelineStartNanos));
+
+        return result;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipeline/CVPipelineResult.java

@@ -0,0 +1,40 @@
+package org.photonvision.common.vision.pipeline;
+
+import org.photonvision.common.vision.frame.Frame;
+import org.photonvision.common.vision.opencv.Releasable;
+import org.photonvision.common.vision.target.TrackedTarget;
+
+import java.util.List;
+
+public class CVPipelineResult implements Releasable {
+    private double latencyMillis;
+    public final double processingMillis;
+    public final List<TrackedTarget> targets;
+    public final Frame outputFrame;
+
+    public CVPipelineResult(double processingMillis, List<TrackedTarget> targets, Frame outputFrame) {
+        this.processingMillis = processingMillis;
+        this.targets = targets;
+
+        this.outputFrame = Frame.copyFrom(outputFrame);
+    }
+
+    public boolean hasTargets() {
+        return !targets.isEmpty();
+    }
+
+    public void release() {
+        for (TrackedTarget tt : targets) {
+            tt.release();
+        }
+        outputFrame.release();
+    }
+
+    public double getLatencyMillis() {
+        return latencyMillis;
+    }
+
+    protected void setLatencyMillis(double latencyMillis) {
+        this.latencyMillis = latencyMillis;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipeline/CVPipelineSettings.java

@@ -0,0 +1,69 @@
+package org.photonvision.common.vision.pipeline;
+
+import org.photonvision.common.vision.frame.FrameDivisor;
+import org.photonvision.common.vision.pipe.ImageFlipMode;
+import org.photonvision.common.vision.pipe.ImageRotationMode;
+import com.fasterxml.jackson.annotation.JsonSubTypes;
+import com.fasterxml.jackson.annotation.JsonTypeInfo;
+
+import java.util.Objects;
+
+@JsonTypeInfo(
+        use = JsonTypeInfo.Id.NAME,
+        include = JsonTypeInfo.As.WRAPPER_ARRAY,
+        property = "type")
+@JsonSubTypes({
+    @JsonSubTypes.Type(value = ColoredShapePipelineSettings.class),
+    @JsonSubTypes.Type(value = ReflectivePipelineSettings.class),
+    @JsonSubTypes.Type(value = DriverModePipelineSettings.class)
+})
+public class CVPipelineSettings {
+    public int pipelineIndex = 0;
+    public PipelineType pipelineType = PipelineType.DriverMode;
+    public ImageFlipMode inputImageFlipMode = ImageFlipMode.NONE;
+    public ImageRotationMode inputImageRotationMode = ImageRotationMode.DEG_0;
+    public String pipelineNickname = "New Pipeline";
+    public double cameraExposure = 50.0;
+    public double cameraBrightness = 50.0;
+    public double cameraGain = 50.0;
+    public int cameraVideoModeIndex = 0;
+    public FrameDivisor inputFrameDivisor = FrameDivisor.NONE;
+    public FrameDivisor outputFrameDivisor = FrameDivisor.NONE;
+    public boolean ledMode = false;
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        CVPipelineSettings that = (CVPipelineSettings) o;
+        return pipelineIndex == that.pipelineIndex
+                && Double.compare(that.cameraExposure, cameraExposure) == 0
+                && Double.compare(that.cameraBrightness, cameraBrightness) == 0
+                && Double.compare(that.cameraGain, cameraGain) == 0
+                && cameraVideoModeIndex == that.cameraVideoModeIndex
+                && ledMode == that.ledMode
+                && pipelineType == that.pipelineType
+                && inputImageFlipMode == that.inputImageFlipMode
+                && inputImageRotationMode == that.inputImageRotationMode
+                && pipelineNickname.equals(that.pipelineNickname)
+                && inputFrameDivisor == that.inputFrameDivisor
+                && outputFrameDivisor == that.outputFrameDivisor;
+    }
+
+    @Override
+    public int hashCode() {
+        return Objects.hash(
+                pipelineIndex,
+                pipelineType,
+                inputImageFlipMode,
+                inputImageRotationMode,
+                pipelineNickname,
+                cameraExposure,
+                cameraBrightness,
+                cameraGain,
+                cameraVideoModeIndex,
+                inputFrameDivisor,
+                outputFrameDivisor,
+                ledMode);
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipeline/Calibration3dPipeline.java

@@ -0,0 +1,22 @@
+package org.photonvision.common.vision.pipeline;
+
+import org.photonvision.common.vision.frame.Frame;
+import org.photonvision.common.vision.frame.FrameStaticProperties;
+
+public class Calibration3dPipeline extends CVPipeline<CVPipelineResult, CVPipelineSettings> {
+
+    // TODO: Everything here
+
+    public Calibration3dPipeline() {
+        settings = new CVPipelineSettings();
+    }
+
+    @Override
+    protected void setPipeParams(
+        FrameStaticProperties frameStaticProperties, CVPipelineSettings settings) {}
+
+    @Override
+    protected CVPipelineResult process(Frame frame, CVPipelineSettings settings) {
+        return null;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipeline/ColoredShapePipeline.java

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

photon-server/src/main/java/org/photonvision/common/vision/pipeline/ColoredShapePipelineSettings.java

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

photon-server/src/main/java/org/photonvision/common/vision/pipeline/DriverModePipeline.java

@@ -0,0 +1,61 @@
+package org.photonvision.common.vision.pipeline;
+
+import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.common.vision.frame.Frame;
+import org.photonvision.common.vision.frame.FrameStaticProperties;
+import org.photonvision.common.vision.opencv.CVMat;
+import org.photonvision.common.vision.pipe.impl.Draw2dCrosshairPipe;
+import org.photonvision.common.vision.pipe.impl.ResizeImagePipe;
+import org.photonvision.common.vision.pipe.impl.RotateImagePipe;
+import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
+
+public class DriverModePipeline
+        extends CVPipeline<DriverModePipelineResult, DriverModePipelineSettings> {
+
+    private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
+
+    private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
+
+    private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
+
+    public DriverModePipeline() {
+        settings = new DriverModePipelineSettings();
+    }
+
+    @Override
+    protected void setPipeParams(
+        FrameStaticProperties frameStaticProperties, DriverModePipelineSettings settings) {
+        RotateImagePipe.RotateImageParams rotateImageParams =
+                new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
+        rotateImagePipe.setParams(rotateImageParams);
+
+        ResizeImagePipe.ResizeImageParams resizeImageParams =
+                new ResizeImagePipe.ResizeImageParams(settings.inputFrameDivisor);
+        resizeImagePipe.setParams(resizeImageParams);
+
+        Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
+                new Draw2dCrosshairPipe.Draw2dCrosshairParams(
+                        settings.offsetPointMode, settings.offsetPoint);
+        draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
+    }
+
+    @Override
+    public DriverModePipelineResult process(Frame frame, DriverModePipelineSettings settings) {
+        // apply pipes
+        var rotateImageResult = rotateImagePipe.apply(frame.image.getMat());
+        var resizeImageResult = resizeImagePipe.apply(rotateImageResult.result);
+        var draw2dCrosshairResult =
+                draw2dCrosshairPipe.apply(Pair.of(resizeImageResult.result, List.of()));
+
+        // calculate elapsed nanoseconds
+        long totalNanos =
+                rotateImageResult.nanosElapsed
+                        + resizeImageResult.nanosElapsed
+                        + draw2dCrosshairResult.nanosElapsed;
+
+        return new DriverModePipelineResult(
+                MathUtils.nanosToMillis(totalNanos),
+                new Frame(new CVMat(draw2dCrosshairResult.result), frame.frameStaticProperties));
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipeline/DriverModePipelineResult.java

@@ -0,0 +1,11 @@
+package org.photonvision.common.vision.pipeline;
+
+import org.photonvision.common.vision.frame.Frame;
+
+import java.util.List;
+
+public class DriverModePipelineResult extends CVPipelineResult {
+    public DriverModePipelineResult(double latencyMillis, Frame outputFrame) {
+        super(latencyMillis, List.of(), outputFrame);
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipeline/DriverModePipelineSettings.java

@@ -0,0 +1,16 @@
+package org.photonvision.common.vision.pipeline;
+
+import org.photonvision.common.util.numbers.DoubleCouple;
+import org.photonvision.common.vision.target.RobotOffsetPointMode;
+import com.fasterxml.jackson.annotation.JsonTypeName;
+
+@JsonTypeName("DriverModePipelineSettings")
+public class DriverModePipelineSettings extends CVPipelineSettings {
+    public RobotOffsetPointMode offsetPointMode = RobotOffsetPointMode.None;
+    public DoubleCouple offsetPoint = new DoubleCouple();
+
+    public DriverModePipelineSettings() {
+        super();
+        pipelineType = PipelineType.DriverMode;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipeline/PipelineType.java

@@ -0,0 +1,17 @@
+package org.photonvision.common.vision.pipeline;
+
+@SuppressWarnings("rawtypes")
+public enum PipelineType {
+    Calib3d(-2, Calibration3dPipeline.class),
+    DriverMode(-1, DriverModePipeline.class),
+    Reflective(0, ReflectivePipeline.class),
+    ColoredShape(0, ColoredShapePipeline.class);
+
+    public final int baseIndex;
+    public final Class clazz;
+
+    PipelineType(int baseIndex, Class clazz) {
+        this.baseIndex = baseIndex;
+        this.clazz = clazz;
+    }
+}

photon-server/src/main/java/org/photonvision/common/vision/pipeline/ReflectivePipeline.java

@@ -0,0 +1,231 @@
+package org.photonvision.common.vision.pipeline;
+
+import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.common.vision.frame.Frame;
+import org.photonvision.common.vision.frame.FrameStaticProperties;
+import org.photonvision.common.vision.opencv.CVMat;
+import org.photonvision.common.vision.opencv.Contour;
+import org.photonvision.common.vision.opencv.DualMat;
+import org.photonvision.common.vision.pipe.CVPipeResult;
+import org.photonvision.common.vision.pipe.impl.Collect2dTargetsPipe;
+import org.photonvision.common.vision.pipe.impl.CornerDetectionPipe;
+import org.photonvision.common.vision.pipe.impl.Draw2dContoursPipe;
+import org.photonvision.common.vision.pipe.impl.Draw2dCrosshairPipe;
+import org.photonvision.common.vision.pipe.impl.Draw3dTargetsPipe;
+import org.photonvision.common.vision.pipe.impl.ErodeDilatePipe;
+import org.photonvision.common.vision.pipe.impl.FilterContoursPipe;
+import org.photonvision.common.vision.pipe.impl.FindContoursPipe;
+import org.photonvision.common.vision.pipe.impl.GroupContoursPipe;
+import org.photonvision.common.vision.pipe.impl.HSVPipe;
+import org.photonvision.common.vision.pipe.impl.OutputMatPipe;
+import org.photonvision.common.vision.pipe.impl.RotateImagePipe;
+import org.photonvision.common.vision.pipe.impl.SolvePNPPipe;
+import org.photonvision.common.vision.pipe.impl.SortContoursPipe;
+import org.photonvision.common.vision.pipe.impl.SpeckleRejectPipe;
+import org.photonvision.common.vision.target.PotentialTarget;
+import org.photonvision.common.vision.target.TrackedTarget;
+import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+
+/** Represents a pipeline for tracking retro-reflective targets. */
+public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectivePipelineSettings> {
+
+    private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
+    private final ErodeDilatePipe erodeDilatePipe = new ErodeDilatePipe();
+    private final HSVPipe hsvPipe = new HSVPipe();
+    private final OutputMatPipe outputMatPipe = new OutputMatPipe();
+    private final FindContoursPipe findContoursPipe = new FindContoursPipe();
+    private final SpeckleRejectPipe speckleRejectPipe = new SpeckleRejectPipe();
+    private final FilterContoursPipe filterContoursPipe = new FilterContoursPipe();
+    private final GroupContoursPipe groupContoursPipe = new GroupContoursPipe();
+    private final SortContoursPipe sortContoursPipe = new SortContoursPipe();
+    private final Collect2dTargetsPipe collect2dTargetsPipe = new Collect2dTargetsPipe();
+    private final CornerDetectionPipe cornerDetectionPipe = new CornerDetectionPipe();
+    private final SolvePNPPipe solvePNPPipe = new SolvePNPPipe();
+    private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
+    private final Draw2dContoursPipe draw2dContoursPipe = new Draw2dContoursPipe();
+    private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
+
+    private Mat rawInputMat = new Mat();
+    private DualMat outputMats = new DualMat();
+
+    public ReflectivePipeline() {
+        settings = new ReflectivePipelineSettings();
+    }
+
+    @Override
+    protected void setPipeParams(
+        FrameStaticProperties frameStaticProperties, ReflectivePipelineSettings settings) {
+        RotateImagePipe.RotateImageParams rotateImageParams =
+                new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
+        rotateImagePipe.setParams(rotateImageParams);
+
+        ErodeDilatePipe.ErodeDilateParams erodeDilateParams =
+                new ErodeDilatePipe.ErodeDilateParams(settings.erode, settings.dilate, 5);
+        // TODO: add kernel size to pipeline settings
+        erodeDilatePipe.setParams(erodeDilateParams);
+
+        HSVPipe.HSVParams hsvParams =
+                new HSVPipe.HSVParams(settings.hsvHue, settings.hsvSaturation, settings.hsvValue);
+        hsvPipe.setParams(hsvParams);
+
+        OutputMatPipe.OutputMatParams outputMatParams =
+                new OutputMatPipe.OutputMatParams(settings.outputShowThresholded);
+        outputMatPipe.setParams(outputMatParams);
+
+        FindContoursPipe.FindContoursParams findContoursParams =
+                new FindContoursPipe.FindContoursParams();
+        findContoursPipe.setParams(findContoursParams);
+
+        SpeckleRejectPipe.SpeckleRejectParams speckleRejectParams =
+                new SpeckleRejectPipe.SpeckleRejectParams(settings.contourSpecklePercentage);
+        speckleRejectPipe.setParams(speckleRejectParams);
+
+        FilterContoursPipe.FilterContoursParams filterContoursParams =
+                new FilterContoursPipe.FilterContoursParams(
+                        settings.contourArea,
+                        settings.contourRatio,
+                        settings.contourExtent,
+                        frameStaticProperties);
+        filterContoursPipe.setParams(filterContoursParams);
+
+        GroupContoursPipe.GroupContoursParams groupContoursParams =
+                new GroupContoursPipe.GroupContoursParams(
+                        settings.contourGroupingMode, settings.contourIntersection);
+        groupContoursPipe.setParams(groupContoursParams);
+
+        SortContoursPipe.SortContoursParams sortContoursParams =
+                new SortContoursPipe.SortContoursParams(settings.contourSortMode, frameStaticProperties, 5);
+        sortContoursPipe.setParams(sortContoursParams);
+
+        Collect2dTargetsPipe.Collect2dTargetsParams collect2dTargetsParams =
+                new Collect2dTargetsPipe.Collect2dTargetsParams(
+                        frameStaticProperties,
+                        settings.offsetRobotOffsetMode,
+                        settings.offsetDualLineM,
+                        settings.offsetDualLineB,
+                        settings.offsetCalibrationPoint.toPoint(),
+                        settings.contourTargetOffsetPointEdge,
+                        settings.contourTargetOrientation);
+        collect2dTargetsPipe.setParams(collect2dTargetsParams);
+
+        var params =
+                new CornerDetectionPipe.CornerDetectionPipeParameters(
+                        settings.cornerDetectionStrategy,
+                        settings.cornerDetectionUseConvexHulls,
+                        settings.cornerDetectionExactSideCount,
+                        settings.cornerDetectionSideCount,
+                        settings.cornerDetectionAccuracyPercentage);
+        cornerDetectionPipe.setParams(params);
+
+        Draw2dContoursPipe.Draw2dContoursParams draw2dContoursParams =
+                new Draw2dContoursPipe.Draw2dContoursParams(settings.outputShowMultipleTargets);
+        draw2dContoursPipe.setParams(draw2dContoursParams);
+
+        Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
+                new Draw2dCrosshairPipe.Draw2dCrosshairParams(
+                        settings.offsetRobotOffsetMode, settings.offsetCalibrationPoint);
+        draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
+
+        var draw3dContoursParams =
+                new Draw3dTargetsPipe.Draw3dContoursParams(
+                        settings.cameraCalibration, settings.targetModel);
+        draw3dTargetsPipe.setParams(draw3dContoursParams);
+
+        var solvePNPParams =
+                new SolvePNPPipe.SolvePNPPipeParams(
+                        settings.cameraCalibration, settings.cameraPitch, settings.targetModel);
+        solvePNPPipe.setParams(solvePNPParams);
+    }
+
+    @Override
+    public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings) {
+        setPipeParams(frame.frameStaticProperties, settings);
+
+        long sumPipeNanosElapsed = 0L;
+
+        frame.image.getMat().copyTo(rawInputMat);
+
+        CVPipeResult<Mat> rotateImageResult = rotateImagePipe.apply(frame.image.getMat());
+        sumPipeNanosElapsed += rotateImageResult.nanosElapsed;
+
+        CVPipeResult<Mat> erodeDilateResult = erodeDilatePipe.apply(rotateImageResult.result);
+        sumPipeNanosElapsed += erodeDilateResult.nanosElapsed;
+
+        CVPipeResult<Mat> hsvPipeResult = hsvPipe.apply(erodeDilateResult.result);
+        sumPipeNanosElapsed += hsvPipeResult.nanosElapsed;
+
+        // mat leak fix attempt
+        outputMats.first = rawInputMat;
+        outputMats.second = hsvPipeResult.result;
+
+        CVPipeResult<Mat> outputMatResult = outputMatPipe.apply(outputMats);
+        sumPipeNanosElapsed += outputMatResult.nanosElapsed;
+
+        CVPipeResult<List<Contour>> findContoursResult = findContoursPipe.apply(hsvPipeResult.result);
+        sumPipeNanosElapsed += findContoursResult.nanosElapsed;
+
+        CVPipeResult<List<Contour>> speckleRejectResult =
+                speckleRejectPipe.apply(findContoursResult.result);
+        sumPipeNanosElapsed += speckleRejectResult.nanosElapsed;
+
+        CVPipeResult<List<PotentialTarget>> groupContoursResult =
+                groupContoursPipe.apply(speckleRejectResult.result);
+        sumPipeNanosElapsed += groupContoursResult.nanosElapsed;
+
+        CVPipeResult<List<PotentialTarget>> sortContoursResult =
+                sortContoursPipe.apply(groupContoursResult.result);
+        sumPipeNanosElapsed += sortContoursResult.nanosElapsed;
+
+        CVPipeResult<List<TrackedTarget>> collect2dTargetsResult =
+                collect2dTargetsPipe.apply(sortContoursResult.result);
+        sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed;
+
+        CVPipeResult<List<TrackedTarget>> targetList;
+
+        // 3d stuff
+        if (settings.solvePNPEnabled) {
+            var cornerDetectionResult = cornerDetectionPipe.apply(collect2dTargetsResult.result);
+            sumPipeNanosElapsed += cornerDetectionResult.nanosElapsed;
+
+            var solvePNPResult = solvePNPPipe.apply(cornerDetectionResult.result);
+            sumPipeNanosElapsed += solvePNPResult.nanosElapsed;
+
+            targetList = solvePNPResult;
+        } else {
+            targetList = collect2dTargetsResult;
+        }
+
+        CVPipeResult<Mat> result;
+
+        CVPipeResult<Mat> draw2dCrosshairResult =
+                draw2dCrosshairPipe.apply(Pair.of(outputMatResult.result, targetList.result));
+        sumPipeNanosElapsed += draw2dCrosshairResult.nanosElapsed;
+
+        CVPipeResult<Mat> draw2dContoursResult =
+                draw2dContoursPipe.apply(
+                        Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
+        sumPipeNanosElapsed += draw2dContoursResult.nanosElapsed;
+
+        if (settings.solvePNPEnabled) {
+            result =
+                    draw3dTargetsPipe.apply(
+                            Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
+            sumPipeNanosElapsed += result.nanosElapsed;
+        } else {
+            result = draw2dContoursResult;
+        }
+
+        // TODO: better way?
+        if (settings.outputShowThresholded) {
+            rawInputMat.release();
+        }
+
+        // TODO: Implement all the things
+        return new CVPipelineResult(
+                MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                collect2dTargetsResult.result,
+                new Frame(new CVMat(result.result), frame.frameStaticProperties));
+    }
+}