Add all relevant 2.X files to _2 package, move/port some 2.X classes

chameleon-server/build.gradle

@@ -44,32 +44,32 @@ dependencies {
 
     implementation "com.moandjiezana.toml:toml4j:0.7.2"
 
-    // javacv
-    def withoutJunk = {
-        exclude group: 'org.bytedeco', module: 'artoolkitplus'
-        exclude group: 'org.bytedeco', module: 'artoolkitplus-platform'
-        exclude group: 'org.bytedeco', module: 'flandmark'
-        exclude group: 'org.bytedeco', module: 'flandmark-platform'
-        exclude group: 'org.bytedeco', module: 'flycapture'
-        exclude group: 'org.bytedeco', module: 'flycapture-platform'
-        exclude group: 'org.bytedeco', module: 'leptonica'
-        exclude group: 'org.bytedeco', module: 'leptonica-platform'
-        exclude group: 'org.bytedeco', module: 'libdc1394'
-        exclude group: 'org.bytedeco', module: 'libdc1394-platform'
-        exclude group: 'org.bytedeco', module: 'libfreenect'
-        exclude group: 'org.bytedeco', module: 'libfreenect-platform'
-        exclude group: 'org.bytedeco', module: 'libfreenect2'
-        exclude group: 'org.bytedeco', module: 'libfreenect2-platform'
-        exclude group: 'org.bytedeco', module: 'librealsense'
-        exclude group: 'org.bytedeco', module: 'librealsense-platform'
-        exclude group: 'org.bytedeco', module: 'librealsense2'
-        exclude group: 'org.bytedeco', module: 'librealsense2-platform'
-        exclude group: 'org.bytedeco', module: 'openblas'
-        exclude group: 'org.bytedeco', module: 'openblas-platform'
-        exclude group: 'org.bytedeco', module: 'tesseract'
-        exclude group: 'org.bytedeco', module: 'tesseract-platform'
-    }
-    compile 'org.bytedeco:javacv-platform:1.5.2', withoutJunk
+//    // javacv
+//    def withoutJunk = {
+//        exclude group: 'org.bytedeco', module: 'artoolkitplus'
+//        exclude group: 'org.bytedeco', module: 'artoolkitplus-platform'
+//        exclude group: 'org.bytedeco', module: 'flandmark'
+//        exclude group: 'org.bytedeco', module: 'flandmark-platform'
+//        exclude group: 'org.bytedeco', module: 'flycapture'
+//        exclude group: 'org.bytedeco', module: 'flycapture-platform'
+//        exclude group: 'org.bytedeco', module: 'leptonica'
+//        exclude group: 'org.bytedeco', module: 'leptonica-platform'
+//        exclude group: 'org.bytedeco', module: 'libdc1394'
+//        exclude group: 'org.bytedeco', module: 'libdc1394-platform'
+//        exclude group: 'org.bytedeco', module: 'libfreenect'
+//        exclude group: 'org.bytedeco', module: 'libfreenect-platform'
+//        exclude group: 'org.bytedeco', module: 'libfreenect2'
+//        exclude group: 'org.bytedeco', module: 'libfreenect2-platform'
+//        exclude group: 'org.bytedeco', module: 'librealsense'
+//        exclude group: 'org.bytedeco', module: 'librealsense-platform'
+//        exclude group: 'org.bytedeco', module: 'librealsense2'
+//        exclude group: 'org.bytedeco', module: 'librealsense2-platform'
+//        exclude group: 'org.bytedeco', module: 'openblas'
+//        exclude group: 'org.bytedeco', module: 'openblas-platform'
+//        exclude group: 'org.bytedeco', module: 'tesseract'
+//        exclude group: 'org.bytedeco', module: 'tesseract-platform'
+//    }
+//    compile 'org.bytedeco:javacv-platform:1.5.2', withoutJunk
 
     // wpilib stuff
     implementation "edu.wpi.first.wpiutil:wpiutil-java:$wpilibVersion"

chameleon-server/settings.gradle

@@ -1,2 +1,2 @@
-rootProject.name = 'ChameleonVision'
+rootProject.name = 'ChameleonServer'
 

chameleon-server/src/main/java/com/chameleonvision/_2/Main.java

@@ -0,0 +1,181 @@
+package com.chameleonvision._2;
+
+import com.chameleonvision._2.config.ConfigManager;
+import com.chameleonvision._2.vision.VisionManager;
+import com.chameleonvision._2.web.Server;
+import com.chameleonvision.common.datatransfer.networktables.NetworkTablesManager;
+import com.chameleonvision.common.networking.NetworkManager;
+import com.chameleonvision.common.scripting.ScriptEventType;
+import com.chameleonvision.common.scripting.ScriptManager;
+import com.chameleonvision.common.util.Platform;
+import com.chameleonvision.common.util.math.IPUtils;
+import edu.wpi.cscore.CameraServerCvJNI;
+import edu.wpi.cscore.CameraServerJNI;
+
+import java.io.IOException;
+
+import static com.chameleonvision.common.util.Platform.CurrentPlatform;
+
+public class Main {
+
+    private static final String NT_SERVERMODE_KEY = "--nt-servermode"; // no args for this setting
+    private static final String NT_CLIENTMODESERVER_KEY = "--nt-client-server"; // expects String representing an IP address (hostnames will be rejected!)
+    private static final String NETWORK_MANAGE_KEY = "--unmanage-network"; // no args for this setting
+    private static final String IGNORE_ROOT_KEY = "--ignore-root"; // no args for this setting
+    private static final String TEST_MODE_KEY = "--cv-development";
+    private static final String UI_PORT_KEY = "--ui-port";
+
+    private static final int DEFAULT_PORT = 5800;
+
+    private static boolean ntServerMode = false;
+    private static boolean manageNetwork = true;
+    private static boolean ignoreRoot = false;
+    private static String ntClientModeServer = null;
+    public static boolean testMode = false;
+    public static int uiPort = DEFAULT_PORT;
+
+    private static void handleArgs(String[] args) {
+        for (int i = 0; i < args.length; i++) {
+            var key = args[i].toLowerCase();
+            String value = null;
+
+            // this switch handles arguments with a value. Add any settings with a value here.
+            switch (key) {
+                case NT_CLIENTMODESERVER_KEY:
+                    var potentialValue = args[i + 1];
+                    // ensures this "value" isnt null, blank, nor another argument
+                    if (potentialValue != null && !potentialValue.isBlank() && !potentialValue.startsWith("-") & !potentialValue.startsWith("--")) {
+                        value = potentialValue.toLowerCase();
+                    }
+                    i++; // increment to skip an 'arg' next go-around of for loop, as that would be this value
+                    break;
+                case UI_PORT_KEY:
+                    var potentialPort = args[i + 1];
+                    if (potentialPort != null && !potentialPort.isBlank() && !potentialPort.startsWith("-") & !potentialPort.startsWith("--")) {
+                        value = potentialPort;
+                    }
+                    i++;
+                    break;
+                case NT_SERVERMODE_KEY:
+                case NETWORK_MANAGE_KEY:
+                case IGNORE_ROOT_KEY:
+                case TEST_MODE_KEY:
+                    // nothing
+                    break;
+            }
+
+            // this switch actually handles the arguments.
+            switch (key) {
+                case NT_SERVERMODE_KEY:
+                    ntServerMode = true;
+                    break;
+                case NT_CLIENTMODESERVER_KEY:
+                    if (value != null) {
+                        if (value.equals("localhost")) {
+                            ntClientModeServer = "127.0.0.1";
+                            continue;
+                        }
+
+                        if (IPUtils.isValidIPV4(value)) {
+                            ntClientModeServer = value;
+                            continue;
+                        }
+                    }
+                    System.err.println("Argument for NT Server Host was invalid, defaulting to team number host");
+                    break;
+                case NETWORK_MANAGE_KEY:
+                    manageNetwork = false;
+                    break;
+                case IGNORE_ROOT_KEY:
+                    ignoreRoot = true;
+                    break;
+                case TEST_MODE_KEY:
+                    testMode = true;
+                    break;
+                case UI_PORT_KEY:
+                    if (value != null) {
+                        try {
+                            uiPort = Integer.parseInt(value);
+                        } catch (NumberFormatException e){
+                            System.err.println("ui Port was not a valid number using port 5800");
+                        }
+                    }
+                    break;
+            }
+        }
+    }
+
+    public static void main(String[] args) {
+
+        Runtime.getRuntime().addShutdownHook(new Thread(() -> ScriptManager.queueEvent(ScriptEventType.kProgramExit)));
+
+        if (CurrentPlatform.equals(Platform.UNSUPPORTED)) {
+            System.err.printf("Sorry, this platform is not supported. Give these details to the developers.\n%s\n", CurrentPlatform.toString());
+            return;
+        } else {
+            System.out.printf("Starting Chameleon Vision on platform %s\n", CurrentPlatform.toString());
+        }
+
+        handleArgs(args);
+
+        if (!CurrentPlatform.isRoot) {
+            if (ignoreRoot) {
+                // manageNetwork = false;
+                System.out.println("Ignoring root, network will not be managed!");
+            } else {
+                System.err.println("This program must be run as root!");
+                return;
+            }
+        }
+
+        // Attempt to load the JNI Libraries
+        System.out.println("Loading CameraServer...");
+        try {
+            CameraServerJNI.forceLoad();
+            CameraServerCvJNI.forceLoad();
+        } catch (UnsatisfiedLinkError | IOException e) {
+            if (CurrentPlatform.isWindows()) {
+                System.err.println("Try to download the VC++ Redistributable, https://aka.ms/vs/16/release/vc_redist.x64.exe");
+            }
+            throw new RuntimeException("Failed to load JNI Libraries!");
+        }
+
+        System.out.println("Checking Settings...");
+        ConfigManager.initializeSettings();
+
+        if (!CurrentPlatform.isWindows()) {
+            System.out.println("Initializing Script Manager...");
+            ScriptManager.initialize();
+        } else {
+            System.out.println("Scripts not yet supported on Windows. ScriptEvents will be ignored.");
+        }
+
+        NetworkManager.getInstance().initialize(manageNetwork);
+
+        if (ntServerMode) {
+            NetworkTablesManager.setServerMode();
+        } else {
+            NetworkTablesManager.setClientMode(ntClientModeServer);
+        }
+
+        ScriptManager.queueEvent(ScriptEventType.kProgramInit);
+
+        boolean visionSourcesOk = VisionManager.initializeSources();
+        if (!visionSourcesOk) {
+            System.err.println("No cameras connected!");
+            return;
+        }
+
+        boolean visionProcessesOk = VisionManager.initializeProcesses();
+        if (!visionProcessesOk) {
+            System.err.println("Failed to initialize vision processes!");
+            return;
+        }
+
+        System.out.println("Starting vision processes...");
+        VisionManager.startProcesses();
+
+        System.out.printf("Starting Web server at port %d\n", uiPort);
+        Server.main(uiPort);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraCalibrationConfig.java

@@ -0,0 +1,64 @@
+package com.chameleonvision._2.config;
+
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnore;
+import com.fasterxml.jackson.annotation.JsonIgnoreType;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.Size;
+
+/**
+ * A class that holds a camera matrix and distortion coefficients for a given resolution
+ */
+public class CameraCalibrationConfig {
+    @JsonProperty("resolution") public final Size resolution;
+    @JsonProperty("cameraMatrix") public final JsonMat cameraMatrix;
+    @JsonProperty("distortionCoeffs") public final JsonMat distortionCoeffs;
+    @JsonProperty("squareSize") public final double squareSize;
+
+    @JsonCreator
+    public CameraCalibrationConfig(
+            @JsonProperty("resolution") Size resolution,
+            @JsonProperty("cameraMatrix") JsonMat cameraMatrix,
+            @JsonProperty("distortionCoeffs") JsonMat distortionCoeffs,
+            @JsonProperty("squareSize") double squareSize) {
+        this.resolution = resolution;
+        this.cameraMatrix = cameraMatrix;
+        this.distortionCoeffs = distortionCoeffs;
+        this.squareSize = squareSize;
+    }
+
+    public CameraCalibrationConfig(Size resolution, Mat cameraMatrix, Mat distortionCoeffs, double squareSize) {
+        this.resolution = resolution;
+        this.cameraMatrix = JsonMat.fromMat(cameraMatrix);
+        this.distortionCoeffs = JsonMat.fromMat(distortionCoeffs);
+        this.squareSize = squareSize;
+    }
+
+    @JsonIgnoreType
+    public static class UICameraCalibrationConfig {
+        public final int width;
+        public final int height;
+        public final double[] cameraMatrix;
+        public final double[] distortionCoeffs;
+
+        public UICameraCalibrationConfig(CameraCalibrationConfig config) {
+            width = (int) config.resolution.width;
+            height = (int) config.resolution.height;
+            cameraMatrix = config.cameraMatrix.data;
+            distortionCoeffs = config.distortionCoeffs.data;
+        }
+
+    }
+
+    @JsonIgnore
+    public Mat getCameraMatrixAsMat() {
+        return cameraMatrix.toMat();
+    }
+
+    @JsonIgnore
+    public MatOfDouble getDistortionCoeffsAsMat() {
+        return new MatOfDouble(distortionCoeffs.toMat());
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraConfig.java

@@ -0,0 +1,181 @@
+package com.chameleonvision._2.config;
+
+import com.chameleonvision.common.util.file.FileUtils;
+import com.chameleonvision.common.util.file.JacksonUtils;
+import com.chameleonvision._2.vision.pipeline.CVPipelineSettings;
+
+import java.io.File;
+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.Objects;
+
+public class CameraConfig {
+
+    private static final Path camerasConfigFolderPath = Path.of(ConfigManager.SettingsPath.toString(), "cameras");
+
+    private final CameraJsonConfig preliminaryConfig;
+    private final Path configFolderPath;
+    private final Path configPath;
+    private final Path driverModePath;
+    private final Path calibrationPath;
+    final Path pipelineFolderPath;
+
+    public final PipelineConfig pipelineConfig;
+
+    CameraConfig(CameraJsonConfig config) {
+        preliminaryConfig = config;
+        String cameraConfigName = preliminaryConfig.name.replace(' ', '_');
+        pipelineConfig = new PipelineConfig(this);
+
+        configFolderPath = Path.of(camerasConfigFolderPath.toString(), cameraConfigName);
+        configPath = Path.of(configFolderPath.toString(), "camera.json");
+        driverModePath = Path.of(configFolderPath.toString(), "drivermode.json");
+        calibrationPath = Path.of(configFolderPath.toString(), "calibration.json");
+        pipelineFolderPath = Paths.get(configFolderPath.toString(), "pipelines");
+    }
+
+    public FullCameraConfiguration load() {
+        checkFolder();
+        checkConfig();
+        checkDriverMode();
+        checkCalibration();
+        pipelineConfig.check();
+
+        return new FullCameraConfiguration(loadConfig(), pipelineConfig.load(), loadDriverMode(), loadCalibration(), this);
+    }
+
+    private CameraJsonConfig loadConfig() {
+        CameraJsonConfig config = preliminaryConfig;
+        try {
+            config = JacksonUtils.deserialize(configPath, CameraJsonConfig.class);
+        } catch (IOException e) {
+            System.err.printf("Failed to load camera config: %s - using default.\n", configPath.toString());
+        }
+        return config;
+    }
+
+    private CVPipelineSettings loadDriverMode() {
+        CVPipelineSettings driverMode = new CVPipelineSettings();
+        try {
+            driverMode = JacksonUtils.deserialize(driverModePath, CVPipelineSettings.class);
+        } catch (IOException e) {
+            System.err.println("Failed to load camera drivermode: " + driverModePath.toString());
+        }
+        if (driverMode != null) {
+            driverMode.nickname = "DRIVERMODE";
+            driverMode.index = -1;
+        }
+        return driverMode;
+    }
+
+    private List<CameraCalibrationConfig> loadCalibration() {
+        List<CameraCalibrationConfig> calibrations = new ArrayList<>();
+        try {
+            calibrations = List.of(Objects.requireNonNull(JacksonUtils.deserialize(calibrationPath, CameraCalibrationConfig[].class)));
+        } catch (Exception e) {
+            System.err.println("Failed to load camera calibration: " + driverModePath.toString());
+        }
+        return calibrations;
+    }
+
+    void saveConfig(CameraJsonConfig config) {
+        try {
+            JacksonUtils.serializer(configPath, config, true);
+            FileUtils.setFilePerms(configPath);
+        } catch (IOException e) {
+            System.err.println("Failed to save camera config file: " + configPath.toString());
+        }
+    }
+
+    void savePipelines(List<CVPipelineSettings> pipelines) {
+        pipelineConfig.save(pipelines);
+    }
+
+    public void saveDriverMode(CVPipelineSettings driverMode) {
+        try {
+            JacksonUtils.serializer(driverModePath, driverMode, true);
+            FileUtils.setFilePerms(driverModePath);
+        } catch (IOException e) {
+            System.err.println("Failed to save camera drivermode file: " + driverModePath.toString());
+        }
+    }
+
+
+    public void saveCalibration(List<CameraCalibrationConfig> cal) {
+        CameraCalibrationConfig[] configs = cal.toArray(new CameraCalibrationConfig[0]);
+        try {
+            JacksonUtils.serializer(calibrationPath, configs, true);
+            FileUtils.setFilePerms(calibrationPath);
+        } catch (IOException e) {
+            System.err.println("Failed to save camera calibration file: " + calibrationPath.toString());
+        }
+    }
+
+    void checkFolder() {
+        if (!configFolderExists()) {
+            try {
+                if (!(new File(configFolderPath.toUri()).mkdirs())) {
+                    System.err.println("Failed to create camera config folder: " + configFolderPath.toString());
+                }
+                FileUtils.setFilePerms(configFolderPath);
+            } catch (Exception e) {
+                System.err.println("Failed to create camera config folder: " + configFolderPath.toString());
+            }
+        }
+    }
+
+    private void checkConfig() {
+        if (!configExists()) {
+            try {
+                JacksonUtils.serializer(configPath, preliminaryConfig, true);
+                FileUtils.setFilePerms(configPath);
+            } catch (IOException e) {
+                System.err.println("Failed to create camera config file: " + configPath.toString());
+            }
+        }
+    }
+
+    private void checkDriverMode() {
+        if (!driverModeExists()) {
+            try {
+                CVPipelineSettings newDriverModeSettings = new CVPipelineSettings();
+                newDriverModeSettings.nickname = "DRIVERMODE";
+                JacksonUtils.serializer(driverModePath, newDriverModeSettings, true);
+                FileUtils.setFilePerms(driverModePath);
+            } catch (IOException e) {
+                System.err.println("Failed to create camera drivermode file: " + driverModePath.toString());
+            }
+        }
+    }
+
+    private void checkCalibration() {
+        if (!calibrationExists()) {
+            try {
+                List<CameraCalibrationConfig> calibrations = new ArrayList<>();
+                JacksonUtils.serializer(calibrationPath, calibrations.toArray(), true);
+            } catch (IOException e) {
+                System.err.println("Failed to create camera calibration file: " + calibrationPath.toString());
+            }
+        }
+    }
+
+    private boolean configFolderExists() {
+        return Files.exists(configFolderPath);
+    }
+
+    private boolean configExists() {
+        return configFolderExists() && Files.exists(configPath);
+    }
+
+    private boolean driverModeExists() {
+        return configFolderExists() && Files.exists(driverModePath);
+    }
+
+    private boolean calibrationExists() {
+        return configFolderExists() && Files.exists(calibrationPath);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraJsonConfig.java

@@ -0,0 +1,53 @@
+package com.chameleonvision._2.config;
+
+import com.chameleonvision._2.vision.VisionProcess;
+import com.chameleonvision._2.vision.camera.USBCaptureProperties;
+import com.chameleonvision._2.vision.enums.StreamDivisor;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonProperty;
+
+public class CameraJsonConfig {
+    public final double fov;
+    public final String path;
+    public final String name;
+    public final String nickname;
+    public final double tilt;
+    public final int videomode;
+    public final StreamDivisor streamDivisor;
+
+    @JsonCreator
+    public CameraJsonConfig(
+            @JsonProperty("fov") double fov,
+            @JsonProperty("path") String path,
+            @JsonProperty("name") String name,
+            @JsonProperty("nickname") String nickname,
+            @JsonProperty("videomode") int videomode,
+            @JsonProperty("streamDivisor") StreamDivisor streamDivisor,
+            @JsonProperty("tilt") double tilt) {
+        this.fov = fov;
+        this.path = path;
+        this.name = name;
+        this.nickname = nickname;
+        this.videomode = videomode;
+        this.streamDivisor = streamDivisor;
+        this.tilt = tilt;
+    }
+
+    public CameraJsonConfig(String path, String name) {
+        this.fov = USBCaptureProperties.DEFAULT_FOV;
+        this.path = path;
+        this.name = name;
+        this.nickname = name;
+        this.videomode = 0;
+        this.streamDivisor = StreamDivisor.NONE;
+        this.tilt = 0;
+    }
+
+    public static CameraJsonConfig fromVisionProcess(VisionProcess process) {
+        USBCaptureProperties camProps = process.getCamera().getProperties();
+        int videomode = camProps.getCurrentVideoModeIndex();
+        StreamDivisor streamDivisor = process.cameraStreamer.getDivisor();
+        double tilt = process.getCamera().getProperties().getTilt().getDegrees();
+        return new CameraJsonConfig(camProps.getFOV(), camProps.path, camProps.name, camProps.getNickname(), videomode, streamDivisor, tilt);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/ConfigManager.java

@@ -0,0 +1,126 @@
+package com.chameleonvision._2.config;
+
+import com.chameleonvision._2.util.ProgramDirectoryUtilities;
+import com.chameleonvision._2.vision.pipeline.CVPipelineSettings;
+import com.chameleonvision.common.util.Platform;
+import com.chameleonvision.common.util.ShellExec;
+import com.chameleonvision.common.util.file.FileUtils;
+import com.chameleonvision.common.util.file.JacksonUtils;
+
+import java.io.File;
+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.LinkedHashMap;
+import java.util.List;
+
+public class ConfigManager {
+    private ConfigManager() {
+    }
+
+    public static final Path SettingsPath = Paths.get(ProgramDirectoryUtilities.getProgramDirectory(), "settings");
+    private static final Path settingsFilePath = Paths.get(SettingsPath.toString(), "settings.json");
+
+    private static final LinkedHashMap<String, CameraConfig> cameraConfigs = new LinkedHashMap<>();
+
+    public static GeneralSettings settings = new GeneralSettings();
+
+    private static boolean settingsFolderExists() {
+        return Files.exists(SettingsPath);
+    }
+
+    private static boolean settingsFileExists() {
+        return settingsFolderExists() && Files.exists(settingsFilePath);
+    }
+
+    private static void checkSettingsFolder() {
+        if (!settingsFolderExists()) {
+            try {
+                if (!(new File(SettingsPath.toUri()).mkdirs())) {
+                    System.err.println("Failed to create settings folder: " + SettingsPath.toString());
+                }
+                Files.createDirectory(SettingsPath);
+                if (!Platform.CurrentPlatform.isWindows()) {
+                    new ShellExec().executeBashCommand("sudo chmod -R 0777 " + SettingsPath.toString());
+                }
+            } catch (IOException e) {
+                if (!(e instanceof java.nio.file.FileAlreadyExistsException))
+                    e.printStackTrace();
+            }
+        }
+    }
+
+    private static void checkSettingsFile() {
+        boolean settingsFileEmpty = settingsFileExists() && new File(settingsFilePath.toString()).length() == 0;
+        if (settingsFileEmpty || !settingsFileExists()) {
+            try {
+                JacksonUtils.serializer(settingsFilePath, settings, true);
+                FileUtils.setFilePerms(settingsFilePath);
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+        } else {
+            try {
+                settings = JacksonUtils.deserialize(settingsFilePath, GeneralSettings.class);
+            } catch (IOException e) {
+                System.err.println("Failed to load settings.json, using defaults.");
+            }
+        }
+    }
+
+    public static void initializeSettings() {
+        System.out.println("Settings folder: " + SettingsPath.toString());
+        checkSettingsFolder();
+        checkSettingsFile();
+        FileUtils.setAllPerms(SettingsPath);
+    }
+
+    private static void saveSettingsFile() {
+        try {
+            JacksonUtils.serializer(settingsFilePath, settings, true);
+            FileUtils.setFilePerms(settingsFilePath);
+        } catch (IOException e) {
+            System.err.println("Failed to save settings.json!");
+        }
+    }
+
+    public static void saveGeneralSettings() {
+        checkSettingsFolder();
+        saveSettingsFile();
+    }
+
+    public static List<FullCameraConfiguration> initializeCameras(List<CameraJsonConfig> preliminaryConfigs) {
+        List<FullCameraConfiguration> configList = new ArrayList<>();
+
+        checkSettingsFolder();
+
+        // loop over all the camera names and try to create settings folders for it
+        for (CameraJsonConfig preliminaryConfig : preliminaryConfigs) {
+            CameraConfig cameraConfiguration = new CameraConfig(preliminaryConfig);
+            cameraConfigs.put(preliminaryConfig.name, cameraConfiguration);
+
+            FullCameraConfiguration camJsonConfig = cameraConfiguration.load();
+
+            configList.add(camJsonConfig);
+        }
+
+        return configList;
+    }
+
+    public static void saveCameraConfig(String cameraName, CameraJsonConfig config) {
+        var camConf = cameraConfigs.get(cameraName);
+        camConf.saveConfig(config);
+    }
+
+    public static void saveCameraPipelines(String cameraName, List<CVPipelineSettings> pipelines) {
+        var camConf = cameraConfigs.get(cameraName);
+        camConf.savePipelines(pipelines);
+    }
+
+    public static void saveCameraDriverMode(String cameraName, CVPipelineSettings driverMode) {
+        var camConf = cameraConfigs.get(cameraName);
+        camConf.saveDriverMode(driverMode);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/FullCameraConfiguration.java

@@ -0,0 +1,21 @@
+package com.chameleonvision._2.config;
+
+import com.chameleonvision._2.vision.pipeline.CVPipelineSettings;
+
+import java.util.List;
+
+public class FullCameraConfiguration {
+    public final CameraJsonConfig cameraConfig;
+    public final List<CVPipelineSettings> pipelines;
+    public final CVPipelineSettings driverMode;
+    public final List<CameraCalibrationConfig> calibration;
+    public final CameraConfig fileConfig;
+
+    FullCameraConfiguration(CameraJsonConfig cameraConfig, List<CVPipelineSettings> pipelines, CVPipelineSettings driverMode, List<CameraCalibrationConfig> calibration, CameraConfig fileConfig) {
+        this.cameraConfig = cameraConfig;
+        this.pipelines = pipelines;
+        this.driverMode = driverMode;
+        this.calibration = calibration;
+        this.fileConfig = fileConfig;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/GeneralSettings.java

@@ -0,0 +1,14 @@
+package com.chameleonvision._2.config;
+
+import com.chameleonvision.common.networking.NetworkMode;
+
+public class GeneralSettings {
+	public int teamNumber = 1577;
+	public NetworkMode connectionType = NetworkMode.DHCP;
+	public String ip = "";
+	public String gateway = "";
+	public String netmask = "";
+	public String hostname = "Chameleon-vision";
+	public String currentCamera = "";
+	public Integer currentPipeline = null;
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/JsonMat.java

@@ -0,0 +1,74 @@
+package com.chameleonvision._2.config;
+
+import com.fasterxml.jackson.annotation.JsonProperty;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+
+import java.util.Arrays;
+
+
+public class JsonMat {
+    public final int rows;
+    public final int cols;
+    public final int type;
+    public final double[] data;
+
+    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;
+    }
+
+    public Mat toMat() {
+        return toMat(this);
+    }
+
+    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);
+    }
+
+    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));
+    }
+
+    public static Mat toMat(JsonMat jsonMat) {
+        if (jsonMat.type != CvType.CV_64FC1) return null;
+
+        Mat retMat = new Mat(jsonMat.rows, jsonMat.cols, jsonMat.type);
+        retMat.put(0, 0, jsonMat.data);
+        return retMat;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/PipelineConfig.java

@@ -0,0 +1,144 @@
+package com.chameleonvision._2.config;
+
+import com.chameleonvision._2.config.serializers.StandardCVPipelineSettingsDeserializer;
+import com.chameleonvision._2.config.serializers.StandardCVPipelineSettingsSerializer;
+import com.chameleonvision._2.vision.pipeline.CVPipelineSettings;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings;
+import com.chameleonvision.common.util.file.FileUtils;
+import com.chameleonvision.common.util.file.JacksonUtils;
+
+import java.io.File;
+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;
+
+public class PipelineConfig {
+
+    private final CameraConfig cameraConfig;
+
+    /**
+     * Construct a new PipelineConfig
+     *
+     * @param cameraConfig the CameraConfig (parent folder, kinda?)
+     */
+    PipelineConfig(CameraConfig cameraConfig) {
+        this.cameraConfig = cameraConfig;
+    }
+
+    private void checkFolder() {
+        if (!(new File(cameraConfig.pipelineFolderPath.toUri()).mkdirs())) {
+            if (Files.notExists(cameraConfig.pipelineFolderPath)) {
+                System.err.println("Failed to create pipelines folder.");
+            }
+        }
+        try {
+            FileUtils.setFilePerms(cameraConfig.pipelineFolderPath);
+        } catch (IOException e) {
+            // ignored
+        }
+    }
+
+    private File[] getPipelineFiles() {
+        return new File(cameraConfig.pipelineFolderPath.toUri()).listFiles();
+    }
+
+    private boolean folderHasPipelines() {
+        File[] folderContents = getPipelineFiles();
+        if (folderContents == null) return false;
+        return folderContents.length > 0;
+    }
+
+    void check() {
+        cameraConfig.checkFolder();
+        checkFolder();
+        // Check if there's at least one pipe
+        if (!folderHasPipelines()) {
+            save(new StandardCVPipelineSettings());
+        }
+    }
+
+    private Path getPipelinePath(CVPipelineSettings setting) {
+        String pipelineName = setting.nickname.replace(' ', '_');
+        String fullFileName = pipelineName + ".json";
+        return Path.of(cameraConfig.pipelineFolderPath.toString(), fullFileName);
+    }
+
+    private boolean pipelineExists(CVPipelineSettings setting) {
+        return Files.exists(getPipelinePath(setting));
+    }
+
+    public void save(CVPipelineSettings settings) {
+
+        var path = getPipelinePath(settings);
+
+        if (settings instanceof StandardCVPipelineSettings) {
+            try {
+                JacksonUtils.serialize(path, (StandardCVPipelineSettings) settings, StandardCVPipelineSettings.class, new StandardCVPipelineSettingsSerializer(), true);
+                FileUtils.setFilePerms(path);
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+        } else {
+            try {
+                JacksonUtils.serializer(path, settings, true);
+                FileUtils.setFilePerms(path);
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+        }
+    }
+
+    public void save(List<CVPipelineSettings> settings) {
+        for (CVPipelineSettings setting : settings) {
+            save(setting);
+        }
+    }
+
+    public void delete(CVPipelineSettings setting) {
+        if (pipelineExists(setting)) {
+            try {
+                Files.delete(getPipelinePath(setting));
+            } catch (IOException e) {
+                System.err.println("Failed to delete pipeline!");
+            }
+        }
+    }
+
+    public CVPipelineSettings rename(CVPipelineSettings setting, String newName) {
+        if (pipelineExists(setting)) {
+            delete(setting);
+            setting.nickname = newName;
+            save(setting);
+        } else {
+            setting.nickname = newName;
+            save(setting);
+        }
+        return setting;
+    }
+
+    public List<CVPipelineSettings> load() {
+        check(); // TODO: this ensures there will be a default pipeline. is the check later necessary?
+
+        File[] pipelineFiles = getPipelineFiles();
+        List<CVPipelineSettings> deserializedList = new ArrayList<>();
+
+        if (pipelineFiles == null || pipelineFiles.length < 1) {
+            // TODO handle no pipelines to load
+            System.err.println("no pipes to load! loading default");
+        } else {
+            for (File pipelineFile : pipelineFiles) {
+                try {
+                    var pipe = JacksonUtils.deserialize(Paths.get(pipelineFile.getPath()), StandardCVPipelineSettings.class, new StandardCVPipelineSettingsDeserializer());
+                    deserializedList.add(pipe);
+                } catch (IOException e) {
+                    System.err.println("couldn't load cvpipeline2d");
+                }
+            }
+        }
+
+        return deserializedList;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseDeserializer.java

@@ -0,0 +1,130 @@
+package com.chameleonvision._2.config.serializers;
+
+import com.chameleonvision.common.util.numbers.DoubleCouple;
+import com.chameleonvision.common.util.numbers.IntegerCouple;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.type.CollectionType;
+import com.fasterxml.jackson.databind.type.TypeFactory;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point3;
+
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+
+public abstract class BaseDeserializer<T> extends StdDeserializer<T> {
+    protected BaseDeserializer(Class<?> vc) {
+        super(vc);
+    }
+
+    JsonNode baseNode;
+
+    private static final CollectionType numberListColType = TypeFactory.defaultInstance().constructCollectionType(List.class, Number.class);
+    private CollectionType pointListColType = TypeFactory.defaultInstance().constructCollectionType(List.class, Object.class);
+    private static final ObjectMapper mapper = new ObjectMapper();
+    private static boolean nodeGood(JsonNode node) {
+        return node != null && !node.toString().equals("");
+    }
+
+    IntegerCouple getNumberCouple(String name, IntegerCouple defaultValue) throws JsonProcessingException {
+        JsonNode node = baseNode.get(name);
+
+        if (nodeGood(node)) {
+            List<Integer> mapped = mapper.readValue(node.toString(), numberListColType);
+            return new IntegerCouple(mapped.get(0), mapped.get(1));
+        }
+        return defaultValue;
+    }
+
+    DoubleCouple getNumberCouple(String name, DoubleCouple defaultValue) throws JsonProcessingException {
+        JsonNode node = baseNode.get(name);
+
+        if (nodeGood(node)) {
+            List<Double> mapped = mapper.readValue(node.toString(), numberListColType);
+            return new DoubleCouple(mapped.get(0), mapped.get(1));
+        }
+        return defaultValue;
+    }
+
+    List<Number> getNumberList(String name, List<Number> defaultValue) throws JsonProcessingException {
+        JsonNode node = baseNode.get(name);
+
+        if (nodeGood(node)) {
+            return mapper.readValue(node.toString(), numberListColType);
+        }
+        return defaultValue;
+    }
+
+    boolean getBoolean(String name, boolean defaultValue) {
+        JsonNode node = baseNode.get(name);
+
+        if (nodeGood(node)) {
+            return node.booleanValue();
+        }
+
+        return defaultValue;
+    }
+
+    int getInt(String name, int defaultValue) {
+        return (int)  getDouble(name, defaultValue);
+    }
+
+    double getDouble(String name, double defaultValue) {
+        JsonNode node = baseNode.get(name);
+
+        if (nodeGood(node)) {
+            return node.numberValue().doubleValue();
+        }
+
+        return defaultValue;
+    }
+
+    String getString(String name, String defaultValue) {
+        JsonNode node = baseNode.get(name);
+
+        if (nodeGood(node)) {
+            return node.asText();
+        }
+
+        return defaultValue;
+    }
+
+    <E extends Enum<E>> E getEnum(String name, Class<E> enumClass, E defaultValue) throws IOException {
+        JsonNode node = baseNode.get(name);
+
+        if (nodeGood(node)) {
+            E[] possibleVals = enumClass.getEnumConstants();
+            String jsonVal = baseNode.get(name).asText();
+
+            for (E val : possibleVals) {
+                if (val.name().equals(jsonVal)) {
+                    return val;
+                }
+            }
+        }
+
+        return defaultValue;
+    }
+    MatOfPoint3f getMatOfPoint3f(String name, MatOfPoint3f defaultValue) throws JsonProcessingException {
+        JsonNode node = baseNode.get(name);
+        if (nodeGood(node)){
+            List<List<Number>> numberList = mapper.readValue(node.toString(), pointListColType);
+            List<Point3> point3List = new ArrayList<>();
+            for (List<Number> tmp : numberList){
+                Point3 p = new Point3();
+                p.x = tmp.get(0).doubleValue();
+                p.y = tmp.get(1).doubleValue();
+                p.z = tmp.get(2).doubleValue();
+                point3List.add(p);
+            }
+            MatOfPoint3f mat = new MatOfPoint3f();
+            mat.fromList(point3List);
+            return mat;
+        }
+
+        return defaultValue;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseSerializer.java

@@ -0,0 +1,49 @@
+package com.chameleonvision._2.config.serializers;
+
+import com.chameleonvision.common.util.numbers.NumberCouple;
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point3;
+
+import java.io.IOException;
+import java.util.List;
+
+public abstract class BaseSerializer<T> extends StdSerializer<T> {
+    protected BaseSerializer(Class<T> t) {
+        super(t);
+    }
+
+    JsonGenerator generator;
+
+    <N extends NumberCouple> void writeNumberCoupleAsNumberArray(String name, N couple) throws IOException {
+        generator.writeArrayFieldStart(name);
+        generator.writeObject(couple.getFirst());
+        generator.writeObject(couple.getSecond());
+        generator.writeEndArray();
+    }
+
+    void writeNumberListAsNumberArray(String name, List<Number> list) throws IOException {
+        generator.writeArrayFieldStart(name);
+        for (Number i : list) {
+            generator.writeObject(i);
+        }
+        generator.writeEndArray();
+    }
+
+    <E extends Enum<E>> void writeEnum(String name, E num) throws IOException {
+        generator.writeFieldName(name);
+        generator.writeString(num.name());
+    }
+
+    void writeMatOfPoint3f(String name, MatOfPoint3f mat) throws IOException {
+        List<Point3> point3List = mat.toList();
+        generator.writeArrayFieldStart(name);
+
+        for (Point3 point3 : point3List) {
+            double[] tmp = {point3.x, point3.y, point3.z};
+            generator.writeObject(tmp);
+        }
+        generator.writeEndArray();
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsDeserializer.java

@@ -0,0 +1,79 @@
+package com.chameleonvision._2.config.serializers;
+
+import com.chameleonvision._2.vision.enums.*;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationContext;
+
+import java.io.IOException;
+
+public class StandardCVPipelineSettingsDeserializer extends BaseDeserializer<StandardCVPipelineSettings> {
+    public StandardCVPipelineSettingsDeserializer() {
+        this(null);
+    }
+
+    private StandardCVPipelineSettingsDeserializer(Class<?> vc) {
+        super(vc);
+    }
+
+    @Override
+    public StandardCVPipelineSettings deserialize(JsonParser jsonParser, DeserializationContext ctxt) throws IOException, JsonProcessingException {
+        // set BaseDeserializer parser reference.
+        baseNode = jsonParser.getCodec().readTree(jsonParser);
+
+        StandardCVPipelineSettings pipeline = new StandardCVPipelineSettings();
+
+        pipeline.index = getInt("index", pipeline.index);
+
+        pipeline.flipMode = getEnum("flipMode", ImageFlipMode.class, pipeline.flipMode);
+        pipeline.rotationMode = getEnum("rotationMode", ImageRotationMode.class, pipeline.rotationMode);
+
+        pipeline.nickname = getString("nickname", pipeline.nickname);
+
+        pipeline.exposure = getDouble("exposure", pipeline.exposure);
+        pipeline.brightness = getDouble("brightness", pipeline.brightness);
+        pipeline.gain = getDouble("gain", pipeline.gain);
+
+        pipeline.videoModeIndex = getInt("videoModeIndex", pipeline.videoModeIndex);
+
+        pipeline.streamDivisor = getEnum("streamDivisor", StreamDivisor.class, pipeline.streamDivisor);
+
+        pipeline.hue = getNumberCouple("hue", pipeline.hue);
+        pipeline.saturation = getNumberCouple("saturation", pipeline.saturation);
+        pipeline.value = getNumberCouple("value", pipeline.value);
+
+        pipeline.erode = getBoolean("erode", pipeline.erode);
+        pipeline.dilate = getBoolean("dilate", pipeline.dilate);
+
+        pipeline.area = getNumberCouple("area", pipeline.area);
+        pipeline.ratio = getNumberCouple("ratio", pipeline.ratio);
+        pipeline.extent = getNumberCouple("extent", pipeline.extent);
+
+        pipeline.speckle = getInt("speckle", (Integer) pipeline.speckle);
+
+        pipeline.isBinary = getBoolean("isBinary", pipeline.isBinary);
+
+        pipeline.sortMode = getEnum("sortMode", SortMode.class, pipeline.sortMode);
+        pipeline.targetRegion = getEnum("targetRegion", TargetRegion.class, pipeline.targetRegion);
+        pipeline.targetOrientation = getEnum("targetOrientation", TargetOrientation.class, pipeline.targetOrientation);
+
+        pipeline.multiple = getBoolean("multiple", pipeline.multiple);
+
+        pipeline.targetGroup = getEnum("targetGroup", TargetGroup.class, pipeline.targetGroup);
+        pipeline.targetIntersection = getEnum("targetIntersection", TargetIntersection.class, pipeline.targetIntersection);
+
+        pipeline.point = getNumberCouple("point", pipeline.point);
+        
+        pipeline.calibrationMode = getEnum("calibrationMode", CalibrationMode.class, pipeline.calibrationMode);
+        
+        pipeline.dualTargetCalibrationM = getDouble("dualTargetCalibrationM", pipeline.dualTargetCalibrationM);
+        pipeline.dualTargetCalibrationB = getDouble("dualTargetCalibrationB", pipeline.dualTargetCalibrationB);
+
+        pipeline.is3D = getBoolean("is3D", pipeline.is3D);
+        pipeline.targetCornerMat = getMatOfPoint3f("targetCornerMat", pipeline.targetCornerMat);
+        pipeline.accuracy = getDouble("accuracy", pipeline.accuracy.doubleValue());
+
+        return pipeline;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsSerializer.java

@@ -0,0 +1,82 @@
+package com.chameleonvision._2.config.serializers;
+
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings;
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.databind.SerializerProvider;
+
+import java.io.IOException;
+
+public class StandardCVPipelineSettingsSerializer extends BaseSerializer<StandardCVPipelineSettings> {
+    public StandardCVPipelineSettingsSerializer() {
+        this(null);
+    }
+
+    private StandardCVPipelineSettingsSerializer(Class<StandardCVPipelineSettings> t) {
+        super(t);
+    }
+
+    @Override
+    public void serialize(StandardCVPipelineSettings pipeline, JsonGenerator gen, SerializerProvider provider) throws IOException {
+        // set BaseSerializer generator reference.
+        generator = gen;
+
+        gen.writeStartObject();
+
+        gen.writeNumberField("index", pipeline.index);
+
+        writeEnum("flipMode", pipeline.flipMode);
+        writeEnum("rotationMode", pipeline.rotationMode);
+
+        gen.writeStringField("nickname", pipeline.nickname);
+
+        gen.writeNumberField("exposure", pipeline.exposure);
+        gen.writeNumberField("brightness", pipeline.brightness);
+        gen.writeNumberField("gain", pipeline.gain);
+
+        gen.writeNumberField("videoModeIndex", pipeline.videoModeIndex);
+
+        writeEnum("streamDivisor", pipeline.streamDivisor);
+
+        writeNumberCoupleAsNumberArray("hue", pipeline.hue);
+        writeNumberCoupleAsNumberArray("saturation", pipeline.saturation);
+        writeNumberCoupleAsNumberArray("value", pipeline.value);
+
+        gen.writeBooleanField("erode", pipeline.erode);
+        gen.writeBooleanField("dilate", pipeline.dilate);
+
+        writeNumberCoupleAsNumberArray("area", pipeline.area);
+        writeNumberCoupleAsNumberArray("ratio", pipeline.ratio);
+        writeNumberCoupleAsNumberArray("extent", pipeline.extent);
+
+        // speckle rejection
+        gen.writeNumberField("speckle", (Integer) pipeline.speckle);
+
+        // stream output (camera feed, or thresholded feed)
+        gen.writeBooleanField("isBinary", pipeline.isBinary);
+
+        writeEnum("sortMode", pipeline.sortMode);
+        writeEnum("targetRegion", pipeline.targetRegion);
+        writeEnum("targetOrientation", pipeline.targetOrientation);
+
+        // show multiple targets when drawing
+        gen.writeBooleanField("multiple", pipeline.multiple);
+
+        writeEnum("targetGroup", pipeline.targetGroup);
+        writeEnum("targetIntersection", pipeline.targetIntersection);
+
+        // single calibration point
+        writeNumberCoupleAsNumberArray("point", pipeline.point);
+
+        // target X/Y calibration
+        writeEnum("calibrationMode", pipeline.calibrationMode);
+
+        // TODO: better names? or use an array?
+        gen.writeNumberField("dualTargetCalibrationM", pipeline.dualTargetCalibrationM);
+        gen.writeNumberField("dualTargetCalibrationB", pipeline.dualTargetCalibrationB);
+
+        gen.writeBooleanField("is3D", pipeline.is3D);
+        writeMatOfPoint3f("targetCornerMat", pipeline.targetCornerMat);
+        gen.writeNumberField("accuracy", pipeline.accuracy.doubleValue());
+        gen.writeEndObject();
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/network/LinuxNetworking.java

@@ -0,0 +1,103 @@
+package com.chameleonvision._2.network;
+
+import org.apache.commons.io.FileUtils;
+
+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;
+
+public class LinuxNetworking extends SysNetworking {
+    private static final String PATH = "/etc/dhcpcd.conf";
+
+    @Override
+    public boolean setDHCP() {
+        File dhcpConf = new File(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 {
+            System.err.println("dhcpcd5 is not installed cant set ip");
+            return false;
+        }
+        return true;
+    }
+
+    @Override
+    public boolean setHostname(String newHostname) {
+        String[] setHostnameArgs = {"set-hostname", newHostname};
+        try {
+            var setHostnameRetCode = shell.execute("hostnamectl", setHostnameArgs);
+            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 = NetmaskToCIDR.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;
+
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/network/NetmaskToCIDR.java

@@ -0,0 +1,29 @@
+package com.chameleonvision._2.network;
+
+import java.net.InetAddress;
+
+public class NetmaskToCIDR {
+    //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;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkInterface.java

@@ -0,0 +1,54 @@
+package com.chameleonvision._2.network;
+
+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;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkManager.java

@@ -0,0 +1,106 @@
+package com.chameleonvision._2.network;
+
+import com.chameleonvision._2.config.ConfigManager;
+import com.chameleonvision.common.util.Platform;
+
+import java.net.SocketException;
+import java.util.ArrayList;
+import java.util.List;
+
+public class NetworkManager {
+    private NetworkManager() {
+    }
+
+    private static SysNetworking networking;
+    private static boolean isManaged = false;
+
+    public static void initialize(boolean manage) {
+        isManaged = manage;
+        if (!isManaged) {
+            return;
+        }
+
+        Platform platform = Platform.CurrentPlatform;
+
+        if (platform.isLinux()) {
+            networking = new LinuxNetworking();
+        } else if (platform.isWindows()) {
+//			networking = new WindowsNetworking();
+            System.out.println("Windows networking is not yet supported. Running unmanaged.");
+            return;
+        }
+
+        if (networking == null) {
+            throw new RuntimeException("Failed to detect platform!");
+        }
+
+        List<java.net.NetworkInterface> interfaces = new ArrayList<>();
+        List<NetworkInterface> goodInterfaces = new ArrayList<>();
+
+        try {
+            interfaces = networking.getNetworkInterfaces();
+        } catch (SocketException e) {
+            e.printStackTrace();
+        }
+
+        var teamBytes = NetworkManager.GetTeamNumberIPBytes(ConfigManager.settings.teamNumber);
+
+        if (interfaces.size() > 0) {
+            for (var inetface : interfaces) {
+                for (var inetfaceAddr : inetface.getInterfaceAddresses()) {
+                    var rawAddr = inetfaceAddr.getAddress().getAddress();
+                    if (rawAddr.length > 4) continue;
+                    if (rawAddr[1] == teamBytes[0] && rawAddr[2] == teamBytes[1]) {
+                        goodInterfaces.add(new NetworkInterface(inetface, inetfaceAddr));
+                    }
+                }
+            }
+
+            if (goodInterfaces.size() == 0) {
+                isManaged = false;
+                System.err.println("No valid network interfaces found! Staying unmanaged.");
+                return;
+            }
+
+            NetworkInterface botInterface = goodInterfaces.get(0);
+            networking.setNetworkInterface(botInterface);
+        } else {
+            isManaged = false;
+            System.err.println("No valid network interfaces found! Staying unmanaged.");
+        }
+    }
+
+    private static byte[] GetTeamNumberIPBytes(int teamNumber) {
+        return new byte[]{(byte) (teamNumber / 100), (byte) (teamNumber % 100)};
+    }
+
+
+    private static boolean setDHCP() {
+        if (!isManaged) {
+            return true;
+        }
+        return networking.setDHCP();
+    }
+
+    private static boolean setStatic(String ipAddress, String netmask, String gateway) {
+        if (!isManaged) {
+            return true;
+        }
+        return networking.setStatic(ipAddress, netmask, gateway);
+    }
+
+    public static boolean setHostname(String hostname) {
+        if (!isManaged) {
+            return true;
+        }
+        return networking.setHostname(hostname);
+    }
+
+    public static boolean setNetwork(boolean isStatic, String ip, String netmask, String gateway) {
+        if (isStatic) {
+            return setStatic(ip, netmask, gateway);
+        } else {
+            return setDHCP();
+        }
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/network/SysNetworking.java

@@ -0,0 +1,36 @@
+package com.chameleonvision._2.network;
+
+import com.chameleonvision.common.util.ShellExec;
+
+import java.io.IOException;
+import java.net.SocketException;
+import java.util.List;
+
+public abstract class SysNetworking {
+
+	NetworkInterface networkInterface;
+	ShellExec shell = new ShellExec(true, true);
+
+	public String getHostname() {
+		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;
+		}
+	}
+
+	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;
+
+}

chameleon-server/src/main/java/com/chameleonvision/_2/network/WindowsNetworking.java

@@ -0,0 +1,54 @@
+package com.chameleonvision._2.network;
+
+import java.net.SocketException;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+public class WindowsNetworking extends SysNetworking {
+
+	@Override
+	public boolean setDHCP() {
+		return false;
+	}
+
+	@Override
+	public boolean setHostname(String newHostname) {
+		var currentHostname = getHostname();
+
+		if (getHostname() == null) {
+			return false;
+		}
+
+		String command = String.format("wmic computersystem where name=\"%s\" call rename name=\"%s\"", currentHostname, newHostname);
+
+		try {
+			var process = Runtime.getRuntime().exec(command);
+			var returnCode = process.waitFor();
+			return returnCode == 0;
+		} catch(Exception e)  {
+			return false;
+		}
+	}
+
+	@Override
+	public boolean setStatic(String ipAddress, String netmask, String gateway) {
+		return false;
+	}
+
+	@Override
+	public List<java.net.NetworkInterface> getNetworkInterfaces() throws SocketException {
+		var netInterfaces = Collections.list(java.net.NetworkInterface.getNetworkInterfaces());
+
+		List<java.net.NetworkInterface> goodInterfaces = new ArrayList<>();
+
+		for (var netInterface : netInterfaces) {
+			if (netInterface.getDisplayName().toLowerCase().contains("bluetooth")) continue;
+			if (netInterface.getDisplayName().toLowerCase().contains("virtual")) continue;
+			if (netInterface.getDisplayName().toLowerCase().contains("loopback")) continue;
+			if (!netInterface.isUp()) continue;
+			goodInterfaces.add(netInterface);
+		}
+		return goodInterfaces;
+	}
+}

chameleon-server/src/main/java/com/chameleonvision/_2/util/Helpers.java

@@ -0,0 +1,54 @@
+package com.chameleonvision._2.util;
+
+import edu.wpi.cscore.VideoMode;
+
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.io.Writer;
+import java.nio.file.Path;
+import java.util.HashMap;
+
+public class Helpers {
+
+    // TODO: MOVE
+    public static HashMap VideoModeToHashMap(VideoMode videoMode) {
+        return new HashMap<String, Object>() {{
+            put("width", videoMode.width);
+            put("height", videoMode.height);
+            put("fps", videoMode.fps);
+            put("pixelFormat", videoMode.pixelFormat.toString());
+        }};
+    }
+
+
+    // TODO: MOVE
+    private static final String kServicePath = "/etc/systemd/system/chameleonVision.service";
+    private static final String kServiceString = "[Unit]\n" +
+            "Description=chameleon vision\n" +
+            "\n" +
+            "[Service]\n" +
+            "ExecStart=/usr/bin/java -jar %s \n" +
+            "StandardOutput=file:/var/log/chameleon.out.txt\n" +
+            "StandardError=file:/var/log/chameleon.err.txt\n" +
+            "Type=simple\n" +
+            "WorkingDirectory=/usr/local/bin\n" +
+            "\n" +
+            "[Install]\n" +
+            "WantedBy=multi-user.target\n" +
+            "\n";
+
+
+    public static void setService(Path filePath) throws IOException, InterruptedException {
+        String newService = String.format(kServiceString, filePath.toString());
+        File file = new File(kServicePath);
+        if (file.exists()) {
+            file.delete();
+        }
+        Writer writer = new FileWriter(file, false);
+        writer.write(newService);
+        writer.close();
+        Process p = Runtime.getRuntime().exec("systemctl enable chameleonVision.service");
+        p.waitFor();
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/util/ProgramDirectoryUtilities.java

@@ -0,0 +1,37 @@
+package com.chameleonvision._2.util;
+
+import java.io.File;
+import java.net.URISyntaxException;
+
+public class ProgramDirectoryUtilities {
+    private static String getJarName() {
+        return new File(ProgramDirectoryUtilities.class.getProtectionDomain()
+                .getCodeSource()
+                .getLocation()
+                .getPath())
+                .getName();
+    }
+
+    private static boolean runningFromJAR() {
+        String jarName = getJarName();
+        return jarName.contains(".jar");
+    }
+
+    public static String getProgramDirectory() {
+        if (runningFromJAR()) {
+            return getCurrentJARDirectory();
+        } else {
+            return System.getProperty("user.dir");
+        }
+    }
+
+    private static String getCurrentJARDirectory() {
+        try {
+            return new File(ProgramDirectoryUtilities.class.getProtectionDomain().getCodeSource().getLocation().toURI().getPath()).getParent();
+        } catch (URISyntaxException exception) {
+            exception.printStackTrace();
+        }
+
+        return null;
+    }
+}

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

@@ -0,0 +1,187 @@
+package com.chameleonvision._2.vision;
+
+import com.chameleonvision._2.config.*;
+import com.chameleonvision._2.util.Helpers;
+import com.chameleonvision._2.vision.camera.USBCameraCapture;
+import com.chameleonvision._2.vision.pipeline.CVPipeline;
+import com.chameleonvision._2.vision.pipeline.CVPipelineSettings;
+import com.chameleonvision.common.util.Platform;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.UsbCameraInfo;
+import org.opencv.videoio.VideoCapture;
+
+import java.util.*;
+import java.util.stream.Collectors;
+import java.util.stream.Stream;
+
+@SuppressWarnings("rawtypes")
+public class VisionManager {
+    private VisionManager() {}
+
+    private static final LinkedHashMap<String, UsbCameraInfo> usbCameraInfosByCameraName = new LinkedHashMap<>();
+    private static final LinkedList<FullCameraConfiguration> loadedCameraConfigs = new LinkedList<>();
+    private static final LinkedList<VisionProcessManageable> visionProcesses = new LinkedList<>();
+
+    @SuppressWarnings("WeakerAccess")
+    private static class VisionProcessManageable {
+        public final int index;
+        public final String name;
+        public final VisionProcess visionProcess;
+
+        public VisionProcessManageable(int index, String name, VisionProcess visionProcess) {
+            this.index = index;
+            this.name = name;
+            this.visionProcess = visionProcess;
+        }
+    }
+
+    private static VisionProcess currentUIVisionProcess;
+
+    public static boolean initializeSources() {
+        int suffix = 0;
+        for (UsbCameraInfo info : UsbCamera.enumerateUsbCameras()) {
+            VideoCapture cap = new VideoCapture(info.dev);
+            if (cap.isOpened()) {
+                cap.release();
+                // Filter non-ascii characters because ext4 doesn't play nice with unicode in directory names
+                String name = info.name.replaceAll("[^\\x00-\\x7F]", "");
+                while (usbCameraInfosByCameraName.containsKey(name)) {
+                    suffix++;
+                    name = String.format("%s (%d)", name, suffix);
+                }
+                usbCameraInfosByCameraName.put(name, info);
+            }
+        }
+
+        if (usbCameraInfosByCameraName.isEmpty()) {
+            return false;
+        }
+        System.out.printf("[VisionManager] Found %s cameras!\n", usbCameraInfosByCameraName.size());
+
+        // load the config
+        List<CameraJsonConfig> preliminaryConfigs = new ArrayList<>();
+
+        usbCameraInfosByCameraName.forEach((suffixedName, cameraInfo) -> {
+            String truePath;
+
+            if (Platform.CurrentPlatform.isWindows()) {
+                truePath = cameraInfo.path;
+            } else {
+                truePath = Arrays.stream(cameraInfo.otherPaths).filter(x -> x.contains("/dev/v4l/by-path")).findFirst().orElse(cameraInfo.path);
+            }
+
+            preliminaryConfigs.add(new CameraJsonConfig(truePath, suffixedName));
+        });
+
+        loadedCameraConfigs.addAll(ConfigManager.initializeCameras(preliminaryConfigs));
+        System.out.printf("[VisionManager] Loaded %s cameras!\n", loadedCameraConfigs.size());
+        return true;
+    }
+
+    public static boolean initializeProcesses() {
+        for (int i = 0; i < loadedCameraConfigs.size(); i++) {
+            FullCameraConfiguration config = loadedCameraConfigs.get(i);
+
+            CameraJsonConfig cameraJsonConfig = config.cameraConfig;
+
+            USBCameraCapture camera = new USBCameraCapture(config);
+            VisionProcess process = new VisionProcess(camera, config);
+            process.pipelineManager.driverModePipeline.settings = config.driverMode;
+            visionProcesses.add(new VisionProcessManageable(i, cameraJsonConfig.name, process));
+        }
+        currentUIVisionProcess = getVisionProcessByIndex(0);
+        ConfigManager.settings.currentCamera = visionProcesses.get(0).name;
+
+        System.out.printf("[VisionManager] Loaded %s vision processes! Current process: %s\n", visionProcesses.size(), visionProcesses.get(0).name);
+        return true;
+    }
+
+    public static void startProcesses() {
+        visionProcesses.forEach((vpm) -> vpm.visionProcess.start());
+    }
+
+    public static VisionProcess getCurrentUIVisionProcess() {
+        return currentUIVisionProcess;
+    }
+
+    public static CameraConfig getCurrentCameraConfig() {
+        return getCameraConfig(currentUIVisionProcess);
+    }
+
+    public static CameraConfig getCameraConfig(VisionProcess process) {
+        String cameraName = process.getCamera().getProperties().name;
+        return Objects.requireNonNull(loadedCameraConfigs.stream().filter(x -> x.cameraConfig.name.equals(cameraName)).findFirst().orElse(null)).fileConfig;
+    }
+
+    public static void setCurrentProcessByIndex(int processIndex) {
+        if (processIndex > visionProcesses.size() - 1) {
+            return;
+        }
+
+        currentUIVisionProcess = getVisionProcessByIndex(processIndex);
+        ConfigManager.settings.currentCamera = visionProcesses.get(processIndex).name;
+    }
+
+    public static VisionProcess getVisionProcessByIndex(int processIndex) {
+        if (processIndex > visionProcesses.size() - 1) {
+            return null;
+        }
+
+        VisionProcessManageable vpm =  visionProcesses.stream().filter(manageable -> manageable.index == processIndex).findFirst().orElse(null);
+        return vpm != null ? vpm.visionProcess : null;
+    }
+
+    public static List<String> getAllCameraNicknames() {
+        return visionProcesses.stream().map(vpm -> vpm.visionProcess.getCamera()
+                .getProperties().getNickname()).collect(Collectors.toList());
+    }
+
+    public static List<String> getCurrentCameraPipelineNicknames() {
+        return currentUIVisionProcess.pipelineManager.pipelines.stream().map(cvPipeline -> cvPipeline.settings.nickname).collect(Collectors.toList());
+    }
+
+
+    public static void saveAllCameras() {
+        visionProcesses.forEach((vpm) -> {
+            VisionProcess process = vpm.visionProcess;
+            String cameraName = process.getCamera().getProperties().name;
+            Stream<CVPipeline> pipelineStream = process.pipelineManager.pipelines.stream();
+            List<CVPipelineSettings> pipelines = process.pipelineManager.pipelines.stream().map(cvPipeline -> cvPipeline.settings).collect(Collectors.toList());
+            CVPipelineSettings driverMode = process.getDriverModeSettings();
+            CameraJsonConfig config = CameraJsonConfig.fromVisionProcess(process);
+            ConfigManager.saveCameraPipelines(cameraName, pipelines);
+            ConfigManager.saveCameraDriverMode(cameraName, driverMode);
+            ConfigManager.saveCameraConfig(cameraName, config);
+        });
+    }
+
+    private static String getCurrentCameraName() {
+        return currentUIVisionProcess.getCamera().getProperties().name;
+    }
+
+    public static void saveCurrentCameraSettings() {
+        CameraJsonConfig config = CameraJsonConfig.fromVisionProcess(currentUIVisionProcess);
+        ConfigManager.saveCameraConfig(getCurrentCameraName(), config);
+    }
+
+    public static void saveCurrentCameraPipelines() {
+        currentUIVisionProcess.pipelineManager.saveAllPipelines();
+    }
+
+    public static void saveCurrentCameraDriverMode() {
+        currentUIVisionProcess.pipelineManager.saveDriverModeConfig();
+    }
+
+    private static List<HashMap> getCameraResolutionList(USBCameraCapture capture) {
+        return capture.getProperties().getVideoModes().stream().map(Helpers::VideoModeToHashMap).collect(Collectors.toList());
+    }
+
+    public static List<HashMap> getCurrentCameraResolutionList() {
+        return getCameraResolutionList(currentUIVisionProcess.getCamera());
+    }
+
+    public static int getCurrentUIVisionProcessIndex() {
+        VisionProcessManageable vpm = visionProcesses.stream().filter(v -> v.visionProcess == currentUIVisionProcess).findFirst().orElse(null);
+        return vpm != null ? vpm.index : -1;
+    }
+}

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

@@ -0,0 +1,389 @@
+package com.chameleonvision._2.vision;
+
+import com.chameleonvision._2.config.CameraCalibrationConfig;
+import com.chameleonvision._2.config.CameraConfig;
+import com.chameleonvision._2.config.ConfigManager;
+import com.chameleonvision._2.config.FullCameraConfiguration;
+import com.chameleonvision._2.vision.camera.CameraStreamer;
+import com.chameleonvision._2.vision.camera.USBCameraCapture;
+import com.chameleonvision._2.vision.pipeline.CVPipelineResult;
+import com.chameleonvision._2.vision.pipeline.CVPipelineSettings;
+import com.chameleonvision._2.vision.pipeline.PipelineManager;
+import com.chameleonvision._2.vision.pipeline.impl.DriverVisionPipeline;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings;
+import com.chameleonvision._2.web.SocketHandler;
+import com.chameleonvision.common.datatransfer.networktables.NetworkTablesManager;
+import com.chameleonvision.common.scripting.ScriptEventType;
+import com.chameleonvision.common.scripting.ScriptManager;
+import com.chameleonvision.common.util.math.MathUtils;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.networktables.*;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpiutil.CircularBuffer;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.stream.Collectors;
+
+
+@SuppressWarnings("rawtypes")
+public class VisionProcess {
+
+    public final USBCameraCapture cameraCapture;
+    private final VisionProcessRunnable visionRunnable;
+    private final CameraConfig fileConfig;
+    public final CameraStreamer cameraStreamer;
+    public PipelineManager pipelineManager;
+
+    private volatile CVPipelineResult lastPipelineResult;
+
+    // network table stuff
+    private final NetworkTable defaultTable;
+    private NetworkTableInstance tableInstance;
+    private NetworkTableEntry ntPipelineEntry;
+    public NetworkTableEntry ntDriverModeEntry;
+    private int ntDriveModeListenerID;
+    private int ntPipelineListenerID;
+    private NetworkTableEntry ntYawEntry;
+    private NetworkTableEntry ntPitchEntry;
+    private NetworkTableEntry ntAuxListEntry;
+    private NetworkTableEntry ntAreaEntry;
+    private NetworkTableEntry ntLatencyEntry;
+    private NetworkTableEntry ntValidEntry;
+    private NetworkTableEntry ntPoseEntry;
+    private NetworkTableEntry ntFittedHeightEntry;
+    private NetworkTableEntry ntFittedWidthEntry;
+    private NetworkTableEntry ntBoundingHeightEntry;
+    private NetworkTableEntry ntBoundingWidthEntry;
+    private NetworkTableEntry ntTargetRotation;
+
+    private ObjectMapper objectMapper = new ObjectMapper();
+
+    private long lastUIUpdateMs = 0;
+
+    VisionProcess(USBCameraCapture cameraCapture, FullCameraConfiguration config) {
+        this.cameraCapture = cameraCapture;
+
+        fileConfig = config.fileConfig;
+
+        pipelineManager = new PipelineManager(this, config.pipelines);
+
+        // Thread to put frames on the dashboard
+        this.cameraStreamer = new CameraStreamer(cameraCapture, config.cameraConfig.name, pipelineManager.getCurrentPipeline().settings.streamDivisor);
+
+        // Thread to process vision data
+        this.visionRunnable = new VisionProcessRunnable();
+
+        // network table
+        defaultTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraCapture.getProperties().getNickname());
+    }
+
+    public void start() {
+        System.out.printf("[%s Process] Creating network table...\n", getCamera().getProperties().getNickname());
+        initNT(defaultTable);
+
+        System.out.printf("[%s Process] Starting vision thread...\n", getCamera().getProperties().getNickname());
+        var visionThread = new Thread(visionRunnable);
+        visionThread.setName(getCamera().getProperties().name + " - Vision Thread");
+        visionThread.start();
+    }
+
+    /**
+     * Removes the old value change listeners
+     * calls {@link #initNT}
+     *
+     * @param newTable passed to {@link #initNT}
+     */
+    public void resetNT(NetworkTable newTable) {
+        ntDriverModeEntry.removeListener(ntDriveModeListenerID);
+        ntPipelineEntry.removeListener(ntPipelineListenerID);
+        initNT(newTable);
+    }
+
+    public void setCameraNickname(String newName) {
+        getCamera().getProperties().setNickname(newName);
+        NetworkTable camTable = NetworkTablesManager.kRootTable.getSubTable(newName);
+        resetNT(camTable);
+    }
+
+    private void initNT(NetworkTable camTable) {
+        tableInstance = camTable.getInstance();
+        ntPipelineEntry = camTable.getEntry("pipeline");
+        ntDriverModeEntry = camTable.getEntry("driverMode");
+        ntPitchEntry = camTable.getEntry("targetPitch");
+        ntYawEntry = camTable.getEntry("targetYaw");
+        ntAreaEntry = camTable.getEntry("targetArea");
+        ntLatencyEntry = camTable.getEntry("latency");
+        ntValidEntry = camTable.getEntry("isValid");
+        ntAuxListEntry = camTable.getEntry("auxTargets");
+        ntPoseEntry = camTable.getEntry("targetPose");
+        ntFittedHeightEntry = camTable.getEntry("targetFittedHeight");
+        ntFittedWidthEntry = camTable.getEntry("targetFittedWidth");
+        ntBoundingHeightEntry = camTable.getEntry("targetBoundingHeight");
+        ntBoundingWidthEntry = camTable.getEntry("targetBoundingWidth");
+        ntTargetRotation = camTable.getEntry("targetRotation");
+        ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate);
+        ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate);
+        ntDriverModeEntry.setBoolean(false);
+        ntPipelineEntry.setNumber(pipelineManager.getCurrentPipelineIndex());
+        pipelineManager.ntIndexEntry = ntPipelineEntry;
+    }
+
+    private void setDriverMode(EntryNotification driverModeEntryNotification) {
+        setDriverMode(driverModeEntryNotification.value.getBoolean());
+    }
+
+    public void setDriverMode(boolean driverMode) {
+        pipelineManager.setDriverMode(driverMode);
+        ScriptManager.queueEvent(driverMode ? ScriptEventType.kEnterDriverMode : ScriptEventType.kExitDriverMode);
+        SocketHandler.sendFullSettings();
+    }
+
+    /**
+     * Method called by the nt entry listener to update the next pipeline.
+     *
+     * @param notification the notification
+     */
+    private void setPipeline(EntryNotification notification) {
+        var wantedPipelineIndex = (int) notification.value.getDouble();
+        if (pipelineManager.pipelines.size() - 1 < wantedPipelineIndex) {
+            ntPipelineEntry.setDouble(pipelineManager.getCurrentPipelineIndex());
+        } else {
+            pipelineManager.setCurrentPipeline(wantedPipelineIndex);
+        }
+    }
+
+    public void setDriverModeEntry(boolean isDriverMode) {
+        // if it's null, we haven't even started the program yet, so just return
+        // otherwise, set it.
+        if (ntDriverModeEntry != null) {
+            ntDriverModeEntry.setBoolean(isDriverMode);
+        }
+    }
+
+    private void updateUI(CVPipelineResult data) {
+        // 30 "FPS" update rate
+        long currentMillis = System.currentTimeMillis();
+        if (currentMillis - lastUIUpdateMs > 1000 / 30) {
+            lastUIUpdateMs = currentMillis;
+
+
+            if (cameraCapture.getProperties().name.equals(ConfigManager.settings.currentCamera)) {
+                HashMap<String, Object> WebSend = new HashMap<>();
+                HashMap<String, Object> point = new HashMap<>();
+                HashMap<String, Object> pointMap = new HashMap<>();
+                ArrayList<Object> webTargets = new ArrayList<>();
+                List<Double> center = new ArrayList<>();
+
+
+                if (data.hasTarget) {
+                    if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
+                        StandardCVPipeline.StandardCVPipelineResult result = (StandardCVPipeline.StandardCVPipelineResult) data;
+                        StandardCVPipeline.TrackedTarget bestTarget = result.targets.get(0);
+                        try {
+                            if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings).multiple) {
+                                for (var target : result.targets) {
+                                    pointMap = new HashMap<>();
+                                    pointMap.put("pitch", target.pitch);
+                                    pointMap.put("yaw", target.yaw);
+                                    pointMap.put("area", target.area);
+                                    pointMap.put("pose", target.cameraRelativePose);
+                                    webTargets.add(pointMap);
+                                }
+                            } else {
+                                pointMap.put("pitch", bestTarget.pitch);
+                                pointMap.put("yaw", bestTarget.yaw);
+                                pointMap.put("area", bestTarget.area);
+                                pointMap.put("pose", bestTarget.cameraRelativePose);
+                                webTargets.add(pointMap);
+                            }
+                            center.add(bestTarget.minAreaRect.center.x);
+                            center.add(bestTarget.minAreaRect.center.y);
+                        } catch (ClassCastException ignored) {
+                          
+                        }
+                    } else {
+                        pointMap.put("pitch", null);
+                        pointMap.put("yaw", null);
+                        pointMap.put("area", null);
+                        pointMap.put("pose", new Pose2d());
+                        webTargets.add(pointMap);
+                        center.add(null);
+                        center.add(null);
+                    }
+
+                    point.put("fps", visionRunnable.fps);
+                    point.put("targets", webTargets);
+                    point.put("rawPoint", center);
+                } else {
+                    point.put("fps", visionRunnable.fps);
+                }
+                WebSend.put("point", point);
+                SocketHandler.broadcastMessage(WebSend);
+            }
+        }
+    }
+
+    private void updateNetworkTableData(CVPipelineResult data) {
+        ntValidEntry.setBoolean(data.hasTarget);
+        if (data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) {
+            if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
+
+                //noinspection unchecked
+                List<StandardCVPipeline.TrackedTarget> targets = (List<StandardCVPipeline.TrackedTarget>) data.targets;
+                StandardCVPipeline.TrackedTarget bestTarget = targets.get(0);
+                ntLatencyEntry.setDouble(MathUtils.roundTo(data.processTime * 1e-6, 3));
+                ntPitchEntry.setDouble(bestTarget.pitch);
+                ntYawEntry.setDouble(bestTarget.yaw);
+                ntAreaEntry.setDouble(bestTarget.area);
+                ntBoundingHeightEntry.setDouble(bestTarget.boundingRect.height);
+                ntBoundingWidthEntry.setDouble(bestTarget.boundingRect.width);
+                ntFittedHeightEntry.setDouble(bestTarget.minAreaRect.size.height);
+                ntFittedWidthEntry.setDouble(bestTarget.minAreaRect.size.width);
+                ntTargetRotation.setDouble(bestTarget.minAreaRect.angle);
+                try {
+                    Pose2d targetPose = targets.get(0).cameraRelativePose;
+                    double[] targetArray = {targetPose.getTranslation().getX(), targetPose.getTranslation().getY(), targetPose.getRotation().getDegrees()};
+                    ntPoseEntry.setDoubleArray(targetArray);
+//                    ntPoseEntry.setString(objectMapper.writeValueAsString(targets.get(0).cameraRelativePose));
+                    ntAuxListEntry.setString(objectMapper.writeValueAsString(targets.stream()
+                            .map(it -> List.of(it.pitch, it.yaw, it.area, it.boundingRect.width, it.boundingRect.height, it.minAreaRect.size.width, it.minAreaRect.size.height, it.minAreaRect.angle, it.cameraRelativePose))
+                            .collect(Collectors.toList())));
+                } catch (JsonProcessingException e) {
+                    e.printStackTrace();
+                }
+            } else {
+                ntPitchEntry.setDouble(0.0);
+                ntYawEntry.setDouble(0.0);
+                ntAreaEntry.setDouble(0.0);
+                ntLatencyEntry.setDouble(0.0);
+                ntAuxListEntry.setString("");
+            }
+        }
+        tableInstance.flush();
+    }
+
+    public void setVideoMode(VideoMode newMode) {
+        cameraCapture.setVideoMode(newMode);
+        cameraStreamer.setNewVideoMode(newMode);
+    }
+
+    public VideoMode getCurrentVideoMode() {
+        return cameraCapture.getCurrentVideoMode();
+    }
+
+    public List<VideoMode> getPossibleVideoModes() {
+        return cameraCapture.getProperties().videoModes;
+    }
+
+    public USBCameraCapture getCamera() {
+        return cameraCapture;
+    }
+
+    public CVPipelineSettings getDriverModeSettings() {
+        return pipelineManager.driverModePipeline.settings;
+    }
+
+    public void addCalibration(CameraCalibrationConfig cal) {
+        cameraCapture.addCalibrationData(cal);
+        System.out.println("saving to file");
+        fileConfig.saveCalibration(cameraCapture.getAllCalibrationData());
+    }
+
+    public void setIs3d(Boolean value) {
+        var settings = pipelineManager.getCurrentPipeline().settings;
+        if (settings instanceof StandardCVPipelineSettings) {
+            ((StandardCVPipelineSettings) settings).is3D = value;
+        }
+    }
+
+    public boolean getIs3d() {
+        var settings = pipelineManager.getCurrentPipeline().settings;
+        if (settings instanceof StandardCVPipelineSettings) {
+            return ((StandardCVPipelineSettings) settings).is3D;
+        }
+        return false;
+    }
+
+    /**
+     * VisionProcessRunnable will process images as quickly as possible
+     */
+    private class VisionProcessRunnable implements Runnable {
+
+        volatile Double fps = 0.0;
+        private CircularBuffer fpsAveragingBuffer = new CircularBuffer(7);
+
+        @Override
+        public void run() {
+            var lastUpdateTimeNanos = System.nanoTime();
+            var lastStreamTimeMs = System.currentTimeMillis();
+
+            System.out.printf("[%s Process] Vision Process Thread -- first run!\n", getCamera().getProperties().getNickname());
+
+            while (!Thread.interrupted()) {
+
+                // blocking call, will block until camera has a new frame.
+                Pair<Mat, Long> camData = cameraCapture.getFrame();
+
+
+                Mat camFrame = camData.getLeft();
+                if (camFrame.cols() > 0 && camFrame.rows() > 0) {
+                    CVPipelineResult result = null;
+                    try {
+                        result = pipelineManager.getCurrentPipeline().runPipeline(camFrame);
+                    } catch (Exception e) {
+                        System.err.println("Exception in vision process " + getCamera().getProperties().getNickname() + "!");
+                        e.printStackTrace();
+                    }
+
+                    camFrame.release();
+
+                    if (result != null) {
+                        result.setTimestamp(camData.getRight());
+                        lastPipelineResult = result;
+                        updateNetworkTableData(lastPipelineResult);
+                        updateUI(lastPipelineResult);
+                    }
+                }
+
+                try {
+                    var currentTime = System.currentTimeMillis();
+                    if ((currentTime - lastStreamTimeMs) / 1000d > 1.0 / 30.0) {
+                        if(lastPipelineResult != null) {
+                            cameraStreamer.runStream(lastPipelineResult.outputMat);
+                            lastStreamTimeMs = currentTime;
+                            lastPipelineResult.outputMat.release();
+                        } else {
+                            System.err.printf("[%s Process] Last pipeline result was null!\n", getCamera().getProperties().getNickname());
+                        }
+                    }
+
+                } catch (Exception e) {
+//                    Debug.printInfo("Vision running faster than stream.");
+                    System.err.printf("[%s Process] Exception in vision thread!\n", getCamera().getProperties().getNickname());
+                    e.printStackTrace();
+                }
+
+                var deltaTimeNanos = System.nanoTime() - lastUpdateTimeNanos;
+                fpsAveragingBuffer.addFirst(1.0 / (deltaTimeNanos * 1E-09));
+                lastUpdateTimeNanos = System.nanoTime();
+                fps = getAverageFPS();
+            }
+        }
+
+        double getAverageFPS() {
+            var temp = 0.0;
+            for (int i = 0; i < 7; i++) {
+                temp += fpsAveragingBuffer.get(i);
+            }
+            temp /= 7.0;
+            return temp;
+        }
+    }
+}

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

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

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

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

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

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

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

@@ -0,0 +1,142 @@
+package com.chameleonvision._2.vision.camera;
+
+import com.chameleonvision._2.config.CameraCalibrationConfig;
+import com.chameleonvision._2.config.FullCameraConfiguration;
+import com.chameleonvision._2.util.Helpers;
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.VideoException;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.cameraserver.CameraServer;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class USBCameraCapture implements CameraCapture {
+    private final UsbCamera baseCamera;
+    private final CvSink cvSink;
+    private List<CameraCalibrationConfig> calibrationList;
+    private Mat imageBuffer = new Mat();
+    private USBCaptureProperties properties;
+
+    public USBCameraCapture(FullCameraConfiguration fullCameraConfiguration) {
+        var config = fullCameraConfiguration.cameraConfig;
+        this.calibrationList = new ArrayList<>(); //fullCameraConfiguration.calibration;
+        calibrationList.addAll(fullCameraConfiguration.calibration);
+        baseCamera = new UsbCamera(config.name, config.path);
+        cvSink = CameraServer.getInstance().getVideo(baseCamera);
+        try {
+            properties = new USBCaptureProperties(baseCamera, config);
+        } catch(VideoException e) {
+            System.err.println("Camera cannot be found on the saved USB port!" +
+                    " Ensure that the camera has not been plugged into a different USB port, and if so, correct it.");
+            e.printStackTrace();
+        }
+
+        var videoModes = properties.getVideoModes();
+        if(videoModes.size() < 1) {
+            throw new VideoException("0 video modes are valid! Full list provided by camera: \n\n"
+            + Arrays.stream(baseCamera.enumerateVideoModes()).map(Helpers::VideoModeToHashMap).toString()  );
+        }
+
+        int videoMode = properties.videoModes.size() - 1 <= config.videomode ? config.videomode : 0;
+        setVideoMode(videoMode);
+    }
+
+    public CameraCalibrationConfig getCalibration(Size size) {
+        for(var calibration: calibrationList) {
+            if(calibration.resolution.equals(size)) return calibration;
+        }
+        return null;
+    }
+
+    public CameraCalibrationConfig getCalibration(VideoMode mode) {
+        return getCalibration(new Size(mode.width, mode.height));
+    }
+
+    public void addCalibrationData(CameraCalibrationConfig newConfig) {
+        calibrationList.removeIf(c -> newConfig.resolution.height == c.resolution.height && newConfig.resolution.width == c.resolution.width);
+        calibrationList.add(newConfig);
+    }
+
+    @Override
+    public USBCaptureProperties getProperties() {
+        return properties;
+    }
+
+    @Override
+    public VideoMode getCurrentVideoMode() {
+        return baseCamera.getVideoMode();
+    }
+
+    @Override
+    public Pair<Mat, Long> getFrame() {
+        Long deltaTime;
+        // TODO: Why multiply by 1000 here?
+        Mat tempMat = new Mat();
+        deltaTime = cvSink.grabFrame(tempMat) * 1000L;
+//        tempMat = Imgcodecs.imread("C:\\Users\\imadu\\Documents\\GitHub\\chameleon-vision\\chameleon-server\\testimages\\2020\\image.png");
+        tempMat.copyTo(imageBuffer);
+        tempMat.release();
+        return Pair.of(imageBuffer, deltaTime);
+    }
+
+    @Override
+    public void setExposure(int exposure) {
+        try {
+            baseCamera.setExposureManual(exposure);
+        } catch (VideoException e) {
+            System.err.println("Failed to change camera exposure!");
+        }
+    }
+
+    @Override
+    public void setBrightness(int brightness) {
+        try {
+            baseCamera.setBrightness(brightness);
+        } catch (VideoException e) {
+            System.err.println("Failed to change camera brightness!");
+        }
+    }
+
+    @Override
+    public void setVideoMode(VideoMode mode) {
+        try {
+            baseCamera.setVideoMode(mode);
+            properties.updateVideoMode(mode);
+        } catch (VideoException e) {
+            System.err.println("Failed to change camera video mode!");
+        }
+    }
+
+    public void setVideoMode(int index){
+        VideoMode mode = properties.getVideoModes().get(index);
+        setVideoMode(mode);
+    }
+
+    @Override
+    public void setGain(int gain) {
+        if (properties.isPS3Eye) {
+            try {
+                baseCamera.getProperty("gain_automatic").set(0);
+                baseCamera.getProperty("gain").set(gain);
+            } catch (Exception e) {
+                System.err.println("Failed to change camera gain!");
+            }
+        }
+    }
+
+    @Override
+    public CameraCalibrationConfig getCurrentCalibrationData() {
+        return getCalibration(getCurrentVideoMode());
+    }
+
+    @Override
+    public List<CameraCalibrationConfig> getAllCalibrationData() {
+        return calibrationList;
+    }
+}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,5 @@
+package com.chameleonvision._2.vision.enums;
+
+public enum TargetOrientation {
+    Portrait, Landscape
+}

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

@@ -0,0 +1,5 @@
+package com.chameleonvision._2.vision.enums;
+
+public enum TargetRegion {
+    Center, Top, Bottom, Left, Right
+}

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

@@ -0,0 +1,30 @@
+package com.chameleonvision._2.vision.image;
+
+import com.chameleonvision._2.vision.camera.CaptureStaticProperties;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+public class CaptureProperties {
+
+    protected CaptureStaticProperties staticProperties;
+    private Rotation2d tilt = new Rotation2d();
+
+    protected CaptureProperties() {
+    }
+
+    public CaptureProperties(VideoMode videoMode, double fov) {
+        staticProperties = new CaptureStaticProperties(videoMode, fov);
+    }
+    public void setStaticProperties(CaptureStaticProperties staticProperties) {this.staticProperties = staticProperties;}
+    public CaptureStaticProperties getStaticProperties() {
+        return staticProperties;
+    }
+
+    public Rotation2d getTilt() {
+        return tilt;
+    }
+
+    public void setTilt(Rotation2d tilt) {
+        this.tilt = tilt;
+    }
+}

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

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

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

@@ -0,0 +1,87 @@
+package com.chameleonvision._2.vision.image;
+
+import com.chameleonvision._2.config.CameraCalibrationConfig;
+import com.chameleonvision._2.vision.camera.CameraCapture;
+import edu.wpi.cscore.VideoMode;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.imgcodecs.Imgcodecs;
+
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.List;
+
+public class StaticImageCapture implements CameraCapture {
+
+    private Mat image = new Mat();
+    private final VideoMode fakeVideoMode;
+    private final CaptureProperties properties;
+
+    public StaticImageCapture(Path imagePath) {
+        this(imagePath, 70);
+    }
+
+    public StaticImageCapture(Path imagePath, double FOV) {
+        if (!Files.exists(imagePath)) throw new RuntimeException("Invalid path for image!");
+
+        Mat loadedImage = Imgcodecs.imread(imagePath.toString());
+        loadedImage.copyTo(image);
+        if (image.cols() > 0 && image.rows() > 0) {
+            fakeVideoMode = new VideoMode(VideoMode.PixelFormat.kBGR, image.cols(), image.rows(), 60);
+        } else {
+            throw new RuntimeException("Failed to load image!");
+        }
+
+        properties = new CaptureProperties(fakeVideoMode, FOV);
+    }
+
+    @Override
+    public Pair<Mat, Long> getFrame() {
+        return Pair.of(image, System.nanoTime());
+    }
+
+    @Override
+    public CaptureProperties getProperties() {
+        return properties;
+    }
+
+    @Override
+    public VideoMode getCurrentVideoMode() {
+        return fakeVideoMode;
+    }
+
+    @Override
+    public void setExposure(int exposure) {
+        // do nothing
+    }
+
+    @Override
+    public void setBrightness(int brightness) {
+        // do nothing
+    }
+
+    @Override
+    public void setVideoMode(VideoMode mode) {
+        // do nothing
+    }
+
+    @Override
+    public void setVideoMode(int index) {
+        // do nothing
+    }
+
+    @Override
+    public void setGain(int gain) {
+        // do nothing
+    }
+
+    @Override
+    public CameraCalibrationConfig getCurrentCalibrationData() {
+        return null;
+    }
+
+    @Override
+    public List<CameraCalibrationConfig> getAllCalibrationData() {
+        return null;
+    }
+}

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,251 @@
+package com.chameleonvision._2.vision.pipeline;
+
+import com.chameleonvision._2.config.CameraConfig;
+import com.chameleonvision._2.config.ConfigManager;
+import com.chameleonvision._2.vision.VisionManager;
+import com.chameleonvision._2.vision.VisionProcess;
+import com.chameleonvision._2.vision.pipeline.impl.Calibrate3dPipeline;
+import com.chameleonvision._2.vision.pipeline.impl.DriverVisionPipeline;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings;
+import com.chameleonvision._2.web.SocketHandler;
+import com.chameleonvision.common.scripting.ScriptEventType;
+import com.chameleonvision.common.scripting.ScriptManager;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.networktables.NetworkTableEntry;
+
+import java.util.Comparator;
+import java.util.HashMap;
+import java.util.LinkedList;
+import java.util.List;
+
+@SuppressWarnings("WeakerAccess")
+public class PipelineManager {
+
+    private static final int DRIVERMODE_INDEX = -1;
+    private static final int CAL_3D_INDEX = -2;
+
+    public final LinkedList<CVPipeline> pipelines = new LinkedList<>();
+
+    public final CVPipeline driverModePipeline = new DriverVisionPipeline(new CVPipelineSettings());
+    public final Calibrate3dPipeline calib3dPipe = new Calibrate3dPipeline(new StandardCVPipelineSettings());
+
+    private final VisionProcess parentProcess;
+    private int lastPipelineIndex;
+    private int currentPipelineIndex;
+    public NetworkTableEntry ntIndexEntry;
+
+    public PipelineManager(VisionProcess visionProcess, List<CVPipelineSettings> loadedPipelineSettings) {
+        parentProcess = visionProcess;
+        if (loadedPipelineSettings == null || loadedPipelineSettings.size() == 0) {
+            pipelines.add(new StandardCVPipeline("New Pipeline"));
+        } else {
+            for (CVPipelineSettings setting : loadedPipelineSettings) {
+                addInternalPipeline(setting);
+            }
+        }
+        driverModePipeline.initPipeline(visionProcess.getCamera());
+        setCurrentPipeline(0);
+    }
+
+    private void reassignIndexes() {
+        pipelines.sort(IndexComparator);
+        for (int i = 0; i < pipelines.size(); i++) {
+            pipelines.get(i).settings.index = i;
+        }
+    }
+
+    private CameraConfig getConfig(VisionProcess process) {
+        return VisionManager.getCameraConfig(process);
+    }
+
+    private CameraConfig getConfig() {
+        return getConfig(parentProcess);
+    }
+
+    private void savePipelineConfig(CVPipelineSettings setting) {
+        getConfig().pipelineConfig.save(setting);
+    }
+
+    private void deletePipelineConfig(CVPipelineSettings setting) {
+        getConfig().pipelineConfig.delete(setting);
+    }
+
+    private void renamePipelineConfig(CVPipelineSettings setting, String newName) {
+        getConfig().pipelineConfig.rename(setting, newName);
+    }
+
+    public void saveAllPipelines() {
+        pipelines.parallelStream().map(pipeline -> pipeline.settings).forEach(this::savePipelineConfig);
+    }
+
+    private void addInternalPipeline(CVPipelineSettings setting) {
+        if (setting instanceof StandardCVPipelineSettings) {
+            pipelines.add(new StandardCVPipeline((StandardCVPipelineSettings) setting));
+        } else {
+            System.out.println("Non 2D/3D pipelines not supported!");
+        }
+        reassignIndexes();
+    }
+
+    public void setDriverMode(boolean driverMode) {
+        if (driverMode) setCurrentPipeline(DRIVERMODE_INDEX);
+        else setCurrentPipeline(lastPipelineIndex);
+    }
+
+    public void setCalibrationMode(boolean calibrationMode) {
+        setCurrentPipeline((calibrationMode ? CAL_3D_INDEX : lastPipelineIndex));
+    }
+
+    public void enableCalibrationMode(VideoMode mode) {
+        parentProcess.setVideoMode(mode);
+        calib3dPipe.setVideoMode(mode);
+        setCalibrationMode(true);
+    }
+
+    public boolean getDriverMode() {
+        return currentPipelineIndex == DRIVERMODE_INDEX;
+    }
+
+    public int getCurrentPipelineIndex() {
+        return currentPipelineIndex;
+    }
+
+    public CVPipeline getCurrentPipeline() {
+        if (currentPipelineIndex == DRIVERMODE_INDEX) {
+            return driverModePipeline;
+        } else if (currentPipelineIndex <= CAL_3D_INDEX) {
+          return calib3dPipe;
+        } else {
+            return pipelines.get(currentPipelineIndex);
+        }
+    }
+
+    public void setCurrentPipeline(int index) {
+        CVPipeline newPipeline = null;
+
+        if (index == DRIVERMODE_INDEX) {
+            ScriptManager.queueEvent(ScriptEventType.kLEDOff);
+            newPipeline = driverModePipeline;
+
+            // if we're changing into driver mode, try to set the nt entry to true
+            parentProcess.setDriverModeEntry(true);
+        } else if (index == CAL_3D_INDEX) {
+            parentProcess.setDriverModeEntry(true);
+
+            newPipeline = calib3dPipe;
+        } else {
+            if (index < pipelines.size()&&index>=0) {
+                newPipeline = pipelines.get(index);
+
+                // if we're switching out of driver mode, try to set the nt entry to false
+                parentProcess.setDriverModeEntry(false);
+                ScriptManager.queueEvent(ScriptEventType.kLEDOn);
+            }
+            else
+                {
+                    //TODO alert/warn user that pipeline doesnt exsits
+                    System.err.println("Index is out of bounds");
+                }
+        }
+        if (newPipeline != null) {
+            lastPipelineIndex = currentPipelineIndex;
+            currentPipelineIndex = index;
+            getCurrentPipeline().initPipeline(parentProcess.getCamera());
+
+            if (ConfigManager.settings.currentCamera.equals(parentProcess.getCamera().getProperties().name)) {
+                ConfigManager.settings.currentPipeline = currentPipelineIndex;
+
+                HashMap<String, Object> pipeChange = new HashMap<>();
+                pipeChange.put("currentPipeline", currentPipelineIndex);
+                SocketHandler.broadcastMessage(pipeChange);
+                try {
+                    SocketHandler.sendFullSettings();
+                } catch (Exception e) {
+                    // avoid NullPointerException when run before threads start
+                }
+            }
+            newPipeline.initPipeline(parentProcess.getCamera());
+            if (parentProcess.cameraStreamer != null)
+                parentProcess.cameraStreamer.setDivisor(newPipeline.settings.streamDivisor, true);
+            if (ntIndexEntry != null) {
+                ntIndexEntry.setDouble(index);
+            }
+        }
+
+        // gain setting quirk
+        if (!parentProcess.cameraCapture.getProperties().isPS3Eye) {
+            getCurrentPipeline().settings.gain = -1;
+        }
+    }
+
+    public void addPipeline(CVPipelineSettings setting) {
+        addInternalPipeline(setting);
+        savePipelineConfig(setting);
+    }
+
+    public void addPipeline(CVPipeline pipeline) {
+        pipelines.add(pipeline);
+        reassignIndexes();
+        savePipelineConfig(pipeline.settings);
+    }
+
+    public void addNewPipeline(String piplineName) {
+        StandardCVPipeline newPipeline = new StandardCVPipeline();
+        newPipeline.settings.nickname = piplineName;
+        newPipeline.settings.index = pipelines.size();
+        addPipeline(newPipeline);
+    }
+
+    public CVPipeline getPipeline(int index) {
+        return pipelines.get(index);
+    }
+
+    public void duplicatePipeline(CVPipelineSettings pipeline) {
+        duplicatePipeline(pipeline, parentProcess);
+    }
+
+    public void duplicatePipeline(CVPipelineSettings pipeline, VisionProcess destinationProcess) {
+        pipeline.index = destinationProcess.pipelineManager.pipelines.size();
+        pipeline.nickname += "(Copy)";
+        if (destinationProcess.pipelineManager.pipelines.stream().anyMatch(c -> c.settings.nickname.equals(pipeline.nickname))){
+//         throw new DuplicatedKeyException("key Already exists");
+        } else{
+            destinationProcess.pipelineManager.addPipeline(pipeline);
+        }
+    }
+
+    public void renameCurrentPipeline(String newName) {
+        CVPipelineSettings settings = getCurrentPipeline().settings;
+        renamePipelineConfig(settings, newName);
+    }
+
+    public void deleteCurrentPipeline() {
+        deletePipeline(currentPipelineIndex);
+    }
+
+    private void deletePipeline(int index) {
+        if (index == currentPipelineIndex) {
+            currentPipelineIndex -= 1;
+        }
+        deletePipelineConfig(getPipeline(index).settings);
+        pipelines.remove(index);
+        reassignIndexes();
+    }
+
+    public void saveDriverModeConfig() {
+        getConfig().saveDriverMode(driverModePipeline.settings);
+    }
+
+    private static final Comparator<CVPipeline> IndexComparator = (o1, o2) -> {
+        int o1Index = o1.settings.index;
+        int o2Index = o2.settings.index;
+
+        if (o1Index == o2Index) {
+            return 0;
+        } else if (o1Index < o2Index) {
+            return -1;
+        }
+        return 1;
+    };
+}

chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/Calibrate3dPipeline.java

@@ -0,0 +1,170 @@
+package com.chameleonvision._2.vision.pipeline.impl;
+
+import com.chameleonvision._2.config.CameraCalibrationConfig;
+import com.chameleonvision._2.config.ConfigManager;
+import com.chameleonvision._2.vision.pipeline.CVPipeline;
+import com.chameleonvision._2.vision.VisionManager;
+import com.chameleonvision._2.vision.camera.CameraCapture;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.wpilibj.util.Units;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.*;
+import org.opencv.imgproc.Imgproc;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverPipelineResult, StandardCVPipelineSettings> {
+
+    private int checkerboardSquaresHigh = 7;
+    private int checkerboardSquaresWide = 7;
+
+    private MatOfPoint3f objP;// new MatOfPoint3f(checkerboardSquaresHigh + checkerboardSquaresWide, 3);//(checkerboardSquaresWide * checkerboardSquaresHigh, 3);
+    private Size patternSize = new Size(checkerboardSquaresHigh, checkerboardSquaresWide);
+    private Size imageSize;
+    double checkerboardSquareSize = 1; // inches!
+    private MatOfPoint2f calibrationOutput = new MatOfPoint2f();
+    private List<Mat> objpoints = new ArrayList<>();
+    private List<Mat> imgpoints = new ArrayList<>();
+
+    public static double checkerboardSquareSizeUnits = Units.inchesToMeters(1.0);
+
+    public static final int MIN_COUNT = 15;
+    private VideoMode calibrationMode;
+    private final Size windowSize = new Size(11, 11);
+    private final Size zeroZone = new Size(-1, -1);
+    private TermCriteria criteria = new TermCriteria(3, 30, 0.001); //(Imgproc.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+
+    private int captureCount = 0;
+    private double calibrationAccuracy = 0;
+    private boolean wantsSnapshot = false;
+    private double squareSizeInches;
+
+    public Calibrate3dPipeline(StandardCVPipelineSettings settings) {
+        super(settings);
+
+        objP = new MatOfPoint3f();
+
+        for(int i = 0; i < checkerboardSquaresHigh * checkerboardSquaresWide; i++) {
+            objP.push_back(new MatOfPoint3f(new Point3(i / checkerboardSquaresWide, i % checkerboardSquaresHigh, 0.0f)));
+        }
+
+        setSquareSize(checkerboardSquareSizeUnits);
+
+        objpoints.forEach(Mat::release);
+        imgpoints.forEach(Mat::release);
+        objpoints.clear();
+        imgpoints.clear();
+    }
+
+    public void setSquareSize(double size) {
+        this.squareSizeInches = size;
+    }
+
+    public void takeSnapshot() {
+        wantsSnapshot = true;
+    }
+
+    public boolean hasEnoughSnapshots() {
+        return captureCount >= MIN_COUNT - 1;
+    }
+
+    @Override
+    public DriverVisionPipeline.DriverPipelineResult runPipeline(Mat inputMat) {
+
+        // look for checkerboard
+        Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_BGR2GRAY);
+        var checkerboardFound = Calib3d.findChessboardCorners(inputMat, patternSize, calibrationOutput);
+
+
+
+        if(!checkerboardFound) {
+            Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_GRAY2BGR);
+
+            return new DriverVisionPipeline.DriverPipelineResult(null, inputMat, 0);
+        }
+
+//        System.out.println("[SolvePNP] checkerboard found!!");
+
+        // cool we found a checkerboard
+        // do corner subpixel
+        Imgproc.cornerSubPix(inputMat, calibrationOutput, windowSize, zeroZone, criteria);
+
+        // convert back to BGR
+        Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_GRAY2BGR);
+        // draw the chessboard
+        Calib3d.drawChessboardCorners(inputMat, patternSize, calibrationOutput, true);
+
+        if(wantsSnapshot) {
+            this.imageSize = new Size(inputMat.width(), inputMat.height());
+
+            var mat = new MatOfPoint3f();
+            calibrationOutput.copyTo(mat);
+            this.objpoints.add(objP);
+            imgpoints.add(mat);
+            captureCount++;
+            wantsSnapshot = false;
+        }
+
+        imageSize = new Size(inputMat.width(), inputMat.height());
+
+        return new DriverVisionPipeline.DriverPipelineResult(null, inputMat, 0);
+    }
+
+    @Override
+    public void initPipeline(CameraCapture camera) {
+        super.initPipeline(camera);
+        objpoints.clear();
+        imgpoints.clear();
+        captureCount = 0;
+    }
+
+    public boolean tryCalibration() {
+        if (!hasEnoughSnapshots()) return false;
+
+        Mat cameraMatrix = new Mat();
+        Mat distortionCoeffs = new Mat();
+        List<Mat> rvecs = new ArrayList<>();
+        List<Mat> tvecs = new ArrayList<>();
+
+        try {
+           calibrationAccuracy = Calib3d.calibrateCamera(objpoints, imgpoints, imageSize, cameraMatrix, distortionCoeffs, rvecs, tvecs);
+        } catch(Exception e) {
+            System.err.println("Camera calibration failed!");
+            initPipeline(cameraCapture);
+            return false;
+        }
+
+        VideoMode currentVidMode = cameraCapture.getCurrentVideoMode();
+        Size resolution = new Size(currentVidMode.width, currentVidMode.height);
+        CameraCalibrationConfig cal = new CameraCalibrationConfig(resolution, cameraMatrix, distortionCoeffs, squareSizeInches);
+
+        VisionManager.getCurrentUIVisionProcess().addCalibration(cal);
+
+        try {
+            System.out.printf("CALIBRATION SUCCESS (with accuracy %s)! camMatrix: \n%s\ndistortionCoeffs:\n%s\n",
+                    calibrationAccuracy, new ObjectMapper().writeValueAsString(cal.cameraMatrix), new ObjectMapper().writeValueAsString(cal.distortionCoeffs));
+        } catch (JsonProcessingException e) {
+            e.printStackTrace();
+        }
+
+
+        ConfigManager.saveGeneralSettings();
+
+        return true;
+    }
+
+    public void setVideoMode(VideoMode mode){
+        this.calibrationMode = mode;
+    }
+    
+    public int getSnapshotCount() {
+        return captureCount + 1;
+    }
+
+    public double getCalibrationAccuracy(){
+        return calibrationAccuracy;
+    }
+}

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

@@ -0,0 +1,55 @@
+package com.chameleonvision._2.vision.pipeline.impl;
+
+import com.chameleonvision._2.vision.camera.CameraCapture;
+import com.chameleonvision._2.vision.enums.CalibrationMode;
+import com.chameleonvision._2.vision.pipeline.CVPipeline;
+import com.chameleonvision._2.vision.pipeline.CVPipelineResult;
+import com.chameleonvision._2.vision.pipeline.CVPipelineSettings;
+import com.chameleonvision._2.vision.pipeline.pipes.Draw2dCrosshairPipe;
+import com.chameleonvision._2.vision.pipeline.pipes.RotateFlipPipe;
+import com.chameleonvision.common.util.MemoryManager;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+
+import java.util.List;
+
+
+public class DriverVisionPipeline extends CVPipeline<DriverVisionPipeline.DriverPipelineResult, CVPipelineSettings> {
+
+    private RotateFlipPipe rotateFlipPipe;
+    private Draw2dCrosshairPipe drawCrosshairPipe;
+    private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings crosshairPipeSettings = new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings();
+
+    private final MemoryManager memoryManager = new MemoryManager(200, 20000);
+
+    public DriverVisionPipeline(CVPipelineSettings settings) {
+        super(settings);
+        settings.index = -1;
+    }
+
+    @Override
+    public void initPipeline(CameraCapture capture) {
+        super.initPipeline(capture);
+        rotateFlipPipe = new RotateFlipPipe(settings.rotationMode, settings.flipMode);
+        crosshairPipeSettings.showCrosshair=true;
+        drawCrosshairPipe = new Draw2dCrosshairPipe(crosshairPipeSettings, CalibrationMode.None,null,0,0);
+    }
+
+    @Override
+    public DriverPipelineResult runPipeline(Mat inputMat) {
+
+        rotateFlipPipe.setConfig(settings.rotationMode, settings.flipMode);
+
+        Pair<Mat, Long> rotateFlipResult = rotateFlipPipe.run(inputMat);
+        Pair<Mat, Long> draw2dCrosshairResult = drawCrosshairPipe.run(Pair.of(rotateFlipResult.getLeft(),null));
+        memoryManager.run();
+
+        return new DriverPipelineResult(null, draw2dCrosshairResult.getLeft(), 0);
+    }
+
+    public static class DriverPipelineResult extends CVPipelineResult<Void> {
+        public DriverPipelineResult(List<Void> targets, Mat outputMat, long processTime) {
+            super(targets, outputMat, processTime);
+        }
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipeline.java

@@ -0,0 +1,284 @@
+package com.chameleonvision._2.vision.pipeline.impl;
+
+import com.chameleonvision._2.Main;
+import com.chameleonvision._2.vision.camera.CameraCapture;
+import com.chameleonvision._2.vision.camera.CaptureStaticProperties;
+import com.chameleonvision._2.vision.pipeline.CVPipeline;
+import com.chameleonvision._2.vision.pipeline.CVPipelineResult;
+import com.chameleonvision._2.vision.pipeline.pipes.*;
+import com.chameleonvision.common.util.MemoryManager;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Point;
+import org.opencv.core.*;
+
+import java.awt.*;
+import java.util.List;
+
+@SuppressWarnings("WeakerAccess")
+public class StandardCVPipeline extends CVPipeline<StandardCVPipeline.StandardCVPipelineResult, StandardCVPipelineSettings> {
+
+    private Mat rawCameraMat = new Mat();
+
+    private RotateFlipPipe rotateFlipPipe;
+    private BlurPipe blurPipe;
+    private ErodeDilatePipe erodeDilatePipe;
+    private HsvPipe hsvPipe;
+    private FindContoursPipe findContoursPipe;
+    private FilterContoursPipe filterContoursPipe;
+    private SpeckleRejectPipe speckleRejectPipe;
+    private GroupContoursPipe groupContoursPipe;
+    private SortContoursPipe sortContoursPipe;
+    private Collect2dTargetsPipe collect2dTargetsPipe;
+    private Draw2dContoursPipe.Draw2dContoursSettings draw2dContoursSettings;
+    private Draw2dContoursPipe draw2dContoursPipe;
+    private Draw2dCrosshairPipe draw2dCrosshairPipe;
+    private DrawSolvePNPPipe drawSolvePNPPipe;
+    private SolvePNPPipe solvePNPPipe;
+    private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings draw2dCrosshairPipeSettings;
+    private OutputMatPipe outputMatPipe;
+
+    private String pipelineTimeString = "";
+    private CaptureStaticProperties camProps;
+    private Scalar hsvLower, hsvUpper;
+
+    public StandardCVPipeline() {
+        super(new StandardCVPipelineSettings());
+    }
+
+    public StandardCVPipeline(String name) {
+        super(name, new StandardCVPipelineSettings());
+    }
+
+    public StandardCVPipeline(StandardCVPipelineSettings settings) {
+        super(settings);
+    }
+
+    @Override
+    public void initPipeline(CameraCapture process) {
+        super.initPipeline(process);
+
+        camProps = cameraCapture.getProperties().getStaticProperties();
+        hsvLower = new Scalar(settings.hue.getFirst(), settings.saturation.getFirst(), settings.value.getFirst());
+        hsvUpper = new Scalar(settings.hue.getSecond(), settings.saturation.getSecond(), settings.value.getSecond());
+
+        rotateFlipPipe = new RotateFlipPipe(settings.rotationMode, settings.flipMode);
+        blurPipe = new BlurPipe(5);
+        erodeDilatePipe = new ErodeDilatePipe(settings.erode, settings.dilate, 7);
+        hsvPipe = new HsvPipe(hsvLower, hsvUpper);
+        findContoursPipe = new FindContoursPipe();
+        filterContoursPipe = new FilterContoursPipe(settings.area, settings.ratio, settings.extent, camProps);
+        speckleRejectPipe = new SpeckleRejectPipe(settings.speckle.doubleValue());
+        groupContoursPipe = new GroupContoursPipe(settings.targetGroup, settings.targetIntersection);
+        sortContoursPipe = new SortContoursPipe(settings.sortMode, camProps, 5);
+        collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode, settings.targetRegion, settings.targetOrientation, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps);
+        draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings();
+        draw2dCrosshairPipeSettings = new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings();
+
+        draw2dContoursSettings.showCentroid = true;
+        draw2dContoursSettings.centroidColor = new Color(25, 239, 0);
+        draw2dContoursSettings.boxOutlineSize = 2;
+        draw2dContoursSettings.showRotatedBox = true;
+        draw2dContoursSettings.showMaximumBox = true;
+        draw2dContoursSettings.showMultiple = settings.multiple;
+        draw2dCrosshairPipeSettings.showCrosshair = true;
+        draw2dContoursPipe = new Draw2dContoursPipe(draw2dContoursSettings, camProps);
+        draw2dCrosshairPipe = new Draw2dCrosshairPipe(draw2dCrosshairPipeSettings, settings.calibrationMode, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB);
+        outputMatPipe = new OutputMatPipe(settings.isBinary);
+    }
+
+    private final MemoryManager memManager = new MemoryManager(120, 20000);
+
+    private StandardCVPipelineResult resultCache = new StandardCVPipelineResult(List.of(), new Mat(), 0L);
+
+    @Override
+    public StandardCVPipelineResult runPipeline(Mat inputMat) {
+        long totalPipelineTimeNanos = 0;
+        long pipelineStartTimeNanos = System.nanoTime();
+
+        resultCache.release();
+
+        if (cameraCapture == null) {
+            throw new RuntimeException("Pipeline was not initialized before being run!");
+        }
+
+        // TODO (HIGH) find the source of the random NPE
+        if (settings == null) {
+            throw new RuntimeException("settings was not initialized!");
+        }
+        if (inputMat.cols() <= 1) {
+            throw new RuntimeException("Input Mat is empty!");
+        }
+
+        pipelineTimeString = "";
+
+        // prepare pipes
+        camProps = cameraCapture.getProperties().getStaticProperties();
+        hsvLower = new Scalar(settings.hue.getFirst(), settings.saturation.getFirst(), settings.value.getFirst());
+        hsvUpper = new Scalar(settings.hue.getSecond(), settings.saturation.getSecond(), settings.value.getSecond());
+        rotateFlipPipe.setConfig(settings.rotationMode, settings.flipMode);
+        blurPipe.setConfig(0);
+        erodeDilatePipe.setConfig(settings.erode, settings.dilate, 7);
+        hsvPipe.setConfig(hsvLower, hsvUpper);
+        filterContoursPipe.setConfig(settings.area, settings.ratio, settings.extent, camProps);
+        speckleRejectPipe.setConfig(settings.speckle.doubleValue());
+        groupContoursPipe.setConfig(settings.targetGroup, settings.targetIntersection);
+        sortContoursPipe.setConfig(settings.sortMode, camProps, 5);
+        collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode, settings.targetRegion, settings.targetOrientation, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps);
+        draw2dContoursPipe.setConfig(settings.multiple, camProps);
+        draw2dCrosshairPipe.setConfig(draw2dCrosshairPipeSettings, settings.calibrationMode, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB);
+        outputMatPipe.setConfig(settings.isBinary);
+
+        if (settings.is3D) {
+            if (solvePNPPipe == null)
+                solvePNPPipe = new SolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
+            if (drawSolvePNPPipe == null)
+                drawSolvePNPPipe = new DrawSolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData());
+
+            solvePNPPipe.setConfig(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
+            drawSolvePNPPipe.setConfig(cameraCapture.getCurrentCalibrationData());
+            drawSolvePNPPipe.setConfig(settings);
+
+        }
+
+        long pipeInitTimeNanos = System.nanoTime() - pipelineStartTimeNanos;
+
+        // run pipes
+        Pair<Mat, Long> rotateFlipResult = rotateFlipPipe.run(inputMat);
+        totalPipelineTimeNanos += rotateFlipResult.getRight();
+
+        inputMat.copyTo(rawCameraMat);
+
+//        Pair<Mat, Long> blurResult = blurPipe.run(rotateFlipResult.getLeft());
+//        totalPipelineTimeNanos += blurResult.getRight();
+
+        Pair<Mat, Long> erodeDilateResult = erodeDilatePipe.run(rotateFlipResult.getLeft());
+        totalPipelineTimeNanos += erodeDilateResult.getRight();
+
+        Pair<Mat, Long> hsvResult = hsvPipe.run(erodeDilateResult.getLeft());
+        totalPipelineTimeNanos += hsvResult.getRight();
+
+        Pair<List<MatOfPoint>, Long> findContoursResult = findContoursPipe.run(hsvResult.getLeft());
+        totalPipelineTimeNanos += findContoursResult.getRight();
+
+        Pair<List<MatOfPoint>, Long> filterContoursResult = filterContoursPipe.run(findContoursResult.getLeft());
+        totalPipelineTimeNanos += filterContoursResult.getRight();
+
+        Pair<List<MatOfPoint>, Long> speckleRejectResult = speckleRejectPipe.run(filterContoursResult.getLeft());
+        totalPipelineTimeNanos += speckleRejectResult.getRight();
+
+        Pair<List<TrackedTarget>, Long> groupContoursResult = groupContoursPipe.run(speckleRejectResult.getLeft());
+        totalPipelineTimeNanos += groupContoursResult.getRight();
+
+        Pair<List<TrackedTarget>, Long> sortContoursResult = sortContoursPipe.run(groupContoursResult.getLeft());
+        totalPipelineTimeNanos += sortContoursResult.getRight();
+
+        Pair<List<TrackedTarget>, Long> collect2dTargetsResult = collect2dTargetsPipe.run(Pair.of(sortContoursResult.getLeft(), camProps));
+        totalPipelineTimeNanos += collect2dTargetsResult.getRight();
+
+        // takes pair of (Mat of original camera image (8UC3), Mat of HSV thresholded image(8UC1))
+        Pair<Mat, Long> outputMatResult = outputMatPipe.run(Pair.of(rawCameraMat, hsvResult.getLeft()));
+        totalPipelineTimeNanos += outputMatResult.getRight();
+
+        Pair<Mat, Long> result;
+
+        if (!settings.is3D) {
+            // takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
+            result = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft()));
+            totalPipelineTimeNanos += result.getRight();
+        } else {
+            result = outputMatResult;
+        }
+
+        // takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
+        Pair<Mat, Long> draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(result.getLeft(), collect2dTargetsResult.getLeft()));
+        totalPipelineTimeNanos += draw2dCrosshairResult.getRight();
+
+        Mat outputMat;
+
+        if (settings.is3D) {
+            // once we've sorted our targets, perform solvePNP. The number of "best targets" is limited by the above pipe
+            Pair<List<TrackedTarget>, Long> solvePNPResult = solvePNPPipe.run(Pair.of(collect2dTargetsResult.getLeft(), rotateFlipResult.getLeft()));
+            totalPipelineTimeNanos += solvePNPResult.getRight();
+
+            Pair<Mat, Long> draw3dContoursResult = drawSolvePNPPipe.run(Pair.of(outputMatResult.getLeft(), solvePNPResult.getLeft()));
+            totalPipelineTimeNanos += draw3dContoursResult.getRight();
+
+            outputMat = draw3dContoursResult.getLeft();
+        } else {
+            outputMat = draw2dCrosshairResult.getLeft();
+        }
+
+        if (Main.testMode) {
+            pipelineTimeString += String.format("PipeInit: %.2fms, ", pipeInitTimeNanos / 1000000.0);
+            pipelineTimeString += String.format("RotateFlip: %.2fms, ", rotateFlipResult.getRight() / 1000000.0);
+//            pipelineTimeString += String.format("Blur: %.2fms, ", blurResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("ErodeDilate: %.2fms, ", erodeDilateResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("HSV: %.2fms, ", hsvResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("FindContours: %.2fms, ", findContoursResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("FilterContours: %.2fms, ", filterContoursResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("SpeckleReject: %.2fms, ", speckleRejectResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("GroupContours: %.2fms, ", groupContoursResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("SortContours: %.2fms, ", sortContoursResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("Collect2dTargets: %.2fms, ", collect2dTargetsResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("OutputMat: %.2fms, ", outputMatResult.getRight() / 1000000.0);
+            pipelineTimeString += String.format("Draw2dContours: %.2fms, ", result.getRight() / 1000000.0);
+            pipelineTimeString += String.format("Draw2dCrosshair: %.2fms, ", draw2dCrosshairResult.getRight() / 1000000.0);
+
+            System.out.println(pipelineTimeString);
+            double totalPipelineTimeMillis = totalPipelineTimeNanos / 1000000.0;
+            double totalPipelineTimeFPS = 1.0 / (totalPipelineTimeMillis / 1000.0);
+            double truePipelineTimeMillis = (System.nanoTime() - pipelineStartTimeNanos) / 1000000.0;
+            double truePipelineFPS = 1.0 / (truePipelineTimeMillis / 1000.0);
+            System.out.printf("Pipeline processed in %.3fms (%.2fFPS), ", totalPipelineTimeMillis, totalPipelineTimeFPS);
+            System.out.printf("full pipeline run time was %.3fms (%.2fFPS)\n", truePipelineTimeMillis, truePipelineFPS);
+        }
+
+//        memManager.run();
+
+        resultCache = new StandardCVPipelineResult(collect2dTargetsResult.getLeft(), outputMat, totalPipelineTimeNanos);
+        return resultCache;
+    }
+
+    public static class StandardCVPipelineResult extends CVPipelineResult<TrackedTarget> {
+        public StandardCVPipelineResult(List<TrackedTarget> targets, Mat outputMat, long processTimeNanos) {
+            super(targets, outputMat, processTimeNanos);
+        }
+
+        public void release() {
+            targets.forEach(TrackedTarget::release);
+            outputMat.release();
+        }
+    }
+
+    public static class TrackedTarget {
+        public double calibratedX = 0.0;
+        public double calibratedY = 0.0;
+        public double pitch = 0.0;
+        public double yaw = 0.0;
+        public double area = 0.0;
+        public Point point = new Point();
+        public RotatedRect minAreaRect;
+        public Rect boundingRect;
+
+        // 3d stuff
+        public Pose2d cameraRelativePose = new Pose2d();
+        public Mat rVector = new Mat();
+        public Mat tVector = new Mat();
+        public MatOfPoint2f imageCornerPoints = new MatOfPoint2f();
+        public Pair<Rect, Rect> leftRightDualTargetPair = null;
+        public Pair<RotatedRect, RotatedRect> leftRightRotatedRect = null;
+        public MatOfPoint2f rawContour = kMat2f;
+        public MatOfPoint2f approxPoly = new MatOfPoint2f();
+
+        public void release() {
+            rVector.release();
+            tVector.release();
+            imageCornerPoints.release();
+        }
+
+        private static final MatOfPoint2f kMat2f = new MatOfPoint2f();
+    }
+
+
+}

chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipelineSettings.java

@@ -0,0 +1,49 @@
+package com.chameleonvision._2.vision.pipeline.impl;
+
+import com.chameleonvision._2.vision.enums.*;
+import com.chameleonvision._2.vision.pipeline.CVPipelineSettings;
+import com.chameleonvision.common.util.numbers.DoubleCouple;
+import com.chameleonvision.common.util.numbers.IntegerCouple;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point3;
+
+public class StandardCVPipelineSettings extends CVPipelineSettings {
+    public IntegerCouple hue = new IntegerCouple(50, 180);
+    public IntegerCouple saturation = new IntegerCouple(50, 255);
+    public IntegerCouple value = new IntegerCouple(50, 255);
+    public boolean erode = false;
+    public boolean dilate = false;
+    public DoubleCouple area = new DoubleCouple(0.0, 100.0);
+    public DoubleCouple ratio = new DoubleCouple(0.0, 20.0);
+    public DoubleCouple extent = new DoubleCouple(0.0, 100.0);
+    public Number speckle = 5;
+    public boolean isBinary = false;
+    public SortMode sortMode = SortMode.Largest;
+    public TargetRegion targetRegion = TargetRegion.Center;
+    public TargetOrientation targetOrientation = TargetOrientation.Landscape;
+    public boolean multiple = false;
+    public TargetGroup targetGroup = TargetGroup.Single;
+    public TargetIntersection targetIntersection = TargetIntersection.Up;
+    public DoubleCouple point = new DoubleCouple();
+    public CalibrationMode calibrationMode = CalibrationMode.None;
+    public double dualTargetCalibrationM = 1;
+    public double dualTargetCalibrationB = 0;
+
+    // 3d stuff
+    public MatOfPoint3f targetCornerMat = new MatOfPoint3f();
+    public Number accuracy = 5;
+    private static MatOfPoint3f hexTargetMat = new MatOfPoint3f(
+            new Point3(-19.625, 0, 0),
+            new Point3(-9.819867, -17, 0),
+            new Point3(9.819867, -17, 0),
+            new Point3(19.625, 0, 0)
+    );
+
+    public StandardCVPipelineSettings() {
+        super();
+        hexTargetMat.copyTo(targetCornerMat);
+    }
+
+
+    public boolean is3D = false;
+}

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

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

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

@@ -0,0 +1,128 @@
+package com.chameleonvision._2.vision.pipeline.pipes;
+
+import com.chameleonvision._2.vision.camera.CaptureStaticProperties;
+import com.chameleonvision._2.vision.enums.CalibrationMode;
+import com.chameleonvision._2.vision.enums.TargetOrientation;
+import com.chameleonvision._2.vision.enums.TargetRegion;
+import com.chameleonvision._2.vision.pipeline.Pipe;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision.common.util.numbers.DoubleCouple;
+import org.apache.commons.lang3.tuple.Pair;
+import org.apache.commons.math3.util.FastMath;
+import org.opencv.core.Point;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, CaptureStaticProperties>, List<StandardCVPipeline.TrackedTarget>> {
+
+
+    private CaptureStaticProperties camProps;
+    private CalibrationMode calibrationMode;
+    private DoubleCouple calibrationPoint;
+    private double calibrationM, calibrationB;
+    private TargetRegion targetRegion;
+    private TargetOrientation targetOrientation;
+    private List<StandardCVPipeline.TrackedTarget> targets = new ArrayList<>();
+    private Point[] vertices = new Point[4];
+
+    public Collect2dTargetsPipe(CalibrationMode calibrationMode, TargetRegion targetRegion, TargetOrientation targetOrientation, DoubleCouple calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
+        setConfig(calibrationMode, targetRegion, targetOrientation, calibrationPoint, calibrationM, calibrationB, camProps);
+    }
+
+    public void setConfig(CalibrationMode calibrationMode, TargetRegion targetRegion, TargetOrientation targetOrientation, DoubleCouple calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
+        this.calibrationMode = calibrationMode;
+        this.calibrationPoint = calibrationPoint;
+        this.calibrationM = calibrationM;
+        this.calibrationB = calibrationB;
+        this.camProps = camProps;
+        this.targetRegion = targetRegion;
+        this.targetOrientation = targetOrientation;
+    }
+
+    @Override
+    public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(Pair<List<StandardCVPipeline.TrackedTarget>, CaptureStaticProperties> inputPair) {
+        long processStartNanos = System.nanoTime();
+
+        targets.clear();
+        var input = inputPair.getLeft();
+        var imageArea = inputPair.getRight().imageArea;
+
+        if (input.size() > 0) {
+            for (var t : input) {
+                t.minAreaRect.points(vertices);
+                Point bl = getMiddle(vertices[0], vertices[1]);
+                Point tl = getMiddle(vertices[1], vertices[2]);
+                Point tr = getMiddle(vertices[2], vertices[3]);
+                Point br = getMiddle(vertices[3], vertices[0]);
+                boolean orientation;
+                if (targetOrientation == TargetOrientation.Landscape) {
+                    orientation = t.minAreaRect.size.width > t.minAreaRect.size.height;
+                } else {
+                    orientation = t.minAreaRect.size.width < t.minAreaRect.size.height;
+                }
+
+                Point result = t.minAreaRect.center;
+                switch (this.targetRegion) {
+                    case Top: {
+                        result = orientation ? tl : tr;
+                        break;
+                    }
+                    case Bottom: {
+                        result = orientation ? br : bl;
+                        break;
+                    }
+                    case Left: {
+                        result = orientation ? bl : tl;
+                        break;
+                    }
+                    case Right: {
+                        result = orientation ? tr : br;
+                        break;
+                    }
+                }
+                t.point = result;
+
+                switch (this.calibrationMode) {
+                    case Single:
+                        if (this.calibrationPoint.equals(new DoubleCouple())) {
+                            this.calibrationPoint.set(camProps.centerX, camProps.centerY);
+                        }
+                        t.calibratedX = this.calibrationPoint.getFirst();
+                        t.calibratedY = this.calibrationPoint.getSecond();
+                        break;
+                    case None:
+                        t.calibratedX = camProps.centerX;
+                        t.calibratedY = camProps.centerY;
+                        break;
+                    case Dual:
+                        t.calibratedX = (t.point.x - this.calibrationB) / this.calibrationM;
+                        t.calibratedY = (t.point.y * this.calibrationM) + this.calibrationB;
+                        break;
+                }
+
+                t.pitch = calculatePitch(t.point.y, t.calibratedY);
+                t.yaw = calculateYaw(t.point.x, t.calibratedX);
+                t.area = t.minAreaRect.size.area() / imageArea;
+
+                targets.add(t);
+            }
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(targets, processTime);
+    }
+
+    private double calculatePitch(double pixelY, double centerY) {
+        double pitch = FastMath.toDegrees(FastMath.atan((pixelY - centerY) / camProps.verticalFocalLength));
+        return (pitch * -1);
+    }
+
+    private double calculateYaw(double pixelX, double centerX) {
+        return FastMath.toDegrees(FastMath.atan((pixelX - centerX) / camProps.horizontalFocalLength));
+    }
+
+    private Point getMiddle(Point p1, Point p2) {
+        return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2));
+    }
+}

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

@@ -0,0 +1,104 @@
+package com.chameleonvision._2.vision.pipeline.pipes;
+
+import com.chameleonvision._2.vision.camera.CaptureStaticProperties;
+import com.chameleonvision._2.vision.pipeline.Pipe;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision.common.util.ColorHelper;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Point;
+import org.opencv.core.*;
+import org.opencv.imgproc.Imgproc;
+
+import java.awt.*;
+import java.util.ArrayList;
+import java.util.List;
+
+public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
+
+    private final Draw2dContoursSettings settings;
+    private CaptureStaticProperties camProps;
+
+    private Mat processBuffer = new Mat();
+    private Mat outputMat = new Mat();
+
+    private Point[] vertices = new Point[4];
+    private List<MatOfPoint> drawnContours = new ArrayList<>();
+    private MatOfPoint contour = new MatOfPoint();
+    @SuppressWarnings("FieldCanBeLocal")
+    private Point xMax = new Point(), xMin = new Point(), yMax = new Point(), yMin = new Point();
+
+
+    public Draw2dContoursPipe(Draw2dContoursSettings settings, CaptureStaticProperties camProps) {
+        this.settings = settings;
+        this.camProps = camProps;
+    }
+
+    public void setConfig(boolean showMultiple,CaptureStaticProperties captureProps) {
+        settings.showMultiple = showMultiple;
+        camProps = captureProps;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> input) {
+        long processStartNanos = System.nanoTime();
+
+        if (settings.showCentroid || settings.showMaximumBox || settings.showRotatedBox) {
+//            input.getLeft().copyTo(processBuffer);
+//            processBuffer = input.getLeft();
+
+            if (input.getRight().size() > 0) {
+                for (int i = 0; i < input.getRight().size(); i++) {
+                    if (i != 0 && !settings.showMultiple){
+                        break;
+                    }
+                    StandardCVPipeline.TrackedTarget target = input.getRight().get(i);
+                    RotatedRect r = input.getRight().get(i).minAreaRect;
+                    if (r == null) continue;
+
+                    drawnContours.forEach(Mat::release);
+                    drawnContours.clear();
+                    drawnContours = new ArrayList<>();
+
+                    r.points(vertices);
+                    contour.fromArray(vertices);
+//                    MatOfPoint contour = new MatOfPoint(vertices);
+                    drawnContours.add(contour);
+
+
+                    if (settings.showRotatedBox) {
+                        Imgproc.drawContours(input.getLeft(), drawnContours, 0, ColorHelper.colorToScalar(settings.rotatedBoxColor), settings.boxOutlineSize);
+                    }
+
+                    if (settings.showMaximumBox) {
+                        Rect box = Imgproc.boundingRect(contour);
+                        Imgproc.rectangle(input.getLeft(), new Point(box.x, box.y), new Point((box.x + box.width), (box.y + box.height)), ColorHelper.colorToScalar(settings.maximumBoxColor), settings.boxOutlineSize);
+                    }
+                    if (settings.showCentroid) {
+                        Imgproc.circle(input.getLeft(), target.point, 3, ColorHelper.colorToScalar(settings.centroidColor), 2);
+
+                    }
+
+//                    contour.release();
+                }
+            }
+
+//            processBuffer.copyTo(outputMat);
+//            processBuffer.release();
+        } else {
+//            input.getLeft().copyTo(outputMat);
+        }
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(input.getLeft(), processTime);
+    }
+
+    public static class Draw2dContoursSettings {
+        public boolean showCentroid = false;
+        public boolean showMultiple = false;
+        public int boxOutlineSize = 0;
+        public boolean showRotatedBox = false;
+        public boolean showMaximumBox = false;
+        public Color centroidColor = Color.GREEN;
+        public Color rotatedBoxColor = Color.BLUE;
+        public Color maximumBoxColor = Color.RED;
+    }
+}

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

@@ -0,0 +1,85 @@
+package com.chameleonvision._2.vision.pipeline.pipes;
+
+import com.chameleonvision._2.vision.enums.CalibrationMode;
+import com.chameleonvision._2.vision.pipeline.Pipe;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision.common.util.ColorHelper;
+import com.chameleonvision.common.util.numbers.DoubleCouple;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.imgproc.Imgproc;
+
+import java.awt.*;
+import java.util.List;
+
+public class Draw2dCrosshairPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
+
+    //Settings
+    private Draw2dCrosshairPipeSettings crosshairSettings;
+    private CalibrationMode calibrationMode;
+    private DoubleCouple calibrationPoint;
+    private double calibrationM, calibrationB;
+
+
+    private Point xMax = new Point(), xMin = new Point(), yMax = new Point(), yMin = new Point();
+
+    public Draw2dCrosshairPipe(Draw2dCrosshairPipeSettings crosshairSettings, CalibrationMode calibrationMode, DoubleCouple calibrationPoint, double calibrationM, double calibrationB) {
+        setConfig(crosshairSettings, calibrationMode, calibrationPoint, calibrationM, calibrationB);
+    }
+
+    public void setConfig(Draw2dCrosshairPipeSettings crosshairSettings, CalibrationMode calibrationMode, DoubleCouple calibrationPoint, double calibrationM, double calibrationB) {
+        this.crosshairSettings = crosshairSettings;
+        this.calibrationMode = calibrationMode;
+        this.calibrationPoint = calibrationPoint;
+        this.calibrationM = calibrationM;
+        this.calibrationB = calibrationB;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> inputPair) {
+        long processStartNanos = System.nanoTime();
+        Mat image = inputPair.getLeft();
+//        List<StandardCVPipeline.TrackedTarget> targets = inputPair.getRight();
+        double x, y;
+        double scale = image.cols() / 32.0;
+
+        drawCrosshair:
+        if (this.crosshairSettings.showCrosshair) {
+            x = image.cols() / 2.0;
+            y = image.rows() / 2.0;
+            switch (this.calibrationMode) {
+                case Single:
+                    if(this.calibrationPoint.equals(new DoubleCouple()))
+                    {
+                        this.calibrationPoint.set(x, y);
+                    }
+                    x = this.calibrationPoint.getFirst().intValue();
+                    y = this.calibrationPoint.getSecond().intValue();
+                    break;
+                case Dual:
+//                    if (targets != null && !targets.isEmpty()) {
+//                        x = targets.get(0).calibratedX;
+//                        y = targets.get(0).calibratedY;
+//                        //TODO dual point calibration crosshair checks
+//                    } else
+//                        break drawCrosshair;
+                    break;
+            }
+            xMax.set(new double[]{x + scale, y});
+            xMin.set(new double[]{x - scale, y});
+            yMax.set(new double[]{x, y + scale});
+            yMin.set(new double[]{x, y - scale});
+            Imgproc.line(inputPair.getLeft(), xMax, xMin, ColorHelper.colorToScalar(this.crosshairSettings.crosshairColor), 2);
+            Imgproc.line(inputPair.getLeft(), yMax, yMin, ColorHelper.colorToScalar(this.crosshairSettings.crosshairColor), 2);
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(inputPair.getLeft(), processTime);
+    }
+
+    public static class Draw2dCrosshairPipeSettings {
+        public boolean showCrosshair = true;
+        public Color crosshairColor = Color.GREEN;
+    }
+}

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

@@ -0,0 +1,126 @@
+package com.chameleonvision._2.vision.pipeline.pipes;
+
+import com.chameleonvision._2.config.CameraCalibrationConfig;
+import com.chameleonvision._2.vision.pipeline.Pipe;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings;
+import com.chameleonvision.common.util.ColorHelper;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.*;
+import org.opencv.core.Point;
+import org.opencv.imgproc.Imgproc;
+
+import java.awt.*;
+import java.util.List;
+import java.util.ArrayList;
+import java.util.stream.Collectors;
+
+public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
+
+    private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
+
+    public Scalar green = ColorHelper.colorToScalar(Color.GREEN);
+    public Scalar blue = ColorHelper.colorToScalar(Color.BLUE);
+    public Scalar red = ColorHelper.colorToScalar(Color.RED);
+    public Scalar orange = ColorHelper.colorToScalar(Color.orange);
+
+    public DrawSolvePNPPipe(StandardCVPipelineSettings standardCVPipelineSettings, CameraCalibrationConfig settings) {
+        setConfig(settings);
+        setBox(standardCVPipelineSettings.targetCornerMat);
+    }
+
+
+    private void setBox(MatOfPoint3f mat) {
+        boxCornerMat.release();
+        var list = mat.toList();
+        var auxList = list.stream().map(it -> new Point3(it.x, it.y, it.z + 6)).collect(Collectors.toList());
+        var finalList = new ArrayList<>(list);
+        finalList.addAll(auxList);
+        boxCornerMat.fromList(finalList);
+    }
+
+
+    public void setConfig(StandardCVPipelineSettings settings) {
+        setBox(settings.targetCornerMat);
+    }
+
+    private Mat cameraMatrix = new Mat();
+    private MatOfDouble distortionCoefficients = new MatOfDouble();
+
+    public void setConfig(CameraCalibrationConfig config) {
+        if (config == null) {
+            System.err.println("got passed a null config! Returning...");
+            return;
+        }
+        setConfig(config.getCameraMatrixAsMat(), config.getDistortionCoeffsAsMat());
+    }
+
+    public void setConfig(Mat cameraMatrix_, MatOfDouble distortionMatrix_) {
+        this.cameraMatrix = cameraMatrix_;
+        this.distortionCoefficients = distortionMatrix_;
+    }
+
+    MatOfPoint2f imagePoints = new MatOfPoint2f();
+
+    @Override
+    public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> targets) {
+        long processStartNanos = System.nanoTime();
+
+        var image = targets.getLeft();
+        for (var it : targets.getRight()) {
+
+            try {
+                Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat(), 0);
+            } catch (Exception e) {
+                e.printStackTrace();
+            }
+            var pts = imagePoints.toList();
+
+            // draw left and right targets if possible
+            if (it.leftRightDualTargetPair != null) {
+                var left = it.leftRightDualTargetPair.getLeft();
+                var right = it.leftRightDualTargetPair.getRight();
+                Imgproc.rectangle(image, left.tl(), left.br(), new Scalar(200, 200, 0), 4);
+                Imgproc.rectangle(image, right.tl(), right.br(), new Scalar(200, 200, 0), 2);
+            }
+
+            // draw poly dp
+            var list = it.approxPoly.toList();
+            for (int i = 0; i < list.size(); i++) {
+                var next = (i == list.size() - 1) ? list.get(0) : list.get(i + 1);
+                Imgproc.line(image, list.get(i), next, red, 2);
+            }
+
+            // draw center
+            Imgproc.circle(image, it.minAreaRect.center, 5, red);
+
+            // draw corners
+            for (int i = 0; i < it.imageCornerPoints.rows(); i++) {
+                var point = new Point(it.imageCornerPoints.get(i, 0));
+                Imgproc.circle(image, point, 4, green, 5);
+            }
+
+            // sketch out floor
+            Imgproc.line(image, pts.get(0), pts.get(1), green, 3);
+            Imgproc.line(image, pts.get(1), pts.get(2), green, 3);
+            Imgproc.line(image, pts.get(2), pts.get(3), green, 3);
+            Imgproc.line(image, pts.get(3), pts.get(0), green, 3);
+
+            // draw pillars
+            Imgproc.line(image, pts.get(0), pts.get(4), blue, 3);
+            Imgproc.line(image, pts.get(1), pts.get(5), blue, 3);
+            Imgproc.line(image, pts.get(2), pts.get(6), blue, 3);
+            Imgproc.line(image, pts.get(3), pts.get(7), blue, 3);
+
+            // draw top
+            Imgproc.line(image, pts.get(4), pts.get(5), red, 3);
+            Imgproc.line(image, pts.get(5), pts.get(6), red, 3);
+            Imgproc.line(image, pts.get(6), pts.get(7), red, 3);
+            Imgproc.line(image, pts.get(7), pts.get(4), red, 3);
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(image, processTime);
+    }
+}

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

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

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

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

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

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

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

@@ -0,0 +1,195 @@
+package com.chameleonvision._2.vision.pipeline.pipes;
+
+import com.chameleonvision._2.vision.enums.TargetGroup;
+import com.chameleonvision._2.vision.enums.TargetIntersection;
+import com.chameleonvision._2.vision.pipeline.Pipe;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision.common.util.math.MathUtils;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.*;
+import org.opencv.imgproc.Imgproc;
+import org.opencv.imgproc.Moments;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
+
+public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCVPipeline.TrackedTarget>> {
+
+    private static final Comparator<MatOfPoint> sortByMomentsX =
+            Comparator.comparingDouble(GroupContoursPipe::calcMomentsX);
+
+    private TargetGroup group;
+    private TargetIntersection intersection;
+
+    private MatOfPoint2f contourBuffer = new MatOfPoint2f();
+
+    private List<StandardCVPipeline.TrackedTarget> groupedContours = new ArrayList<>();
+    private MatOfPoint2f intersectMatA = new MatOfPoint2f();
+    private MatOfPoint2f intersectMatB = new MatOfPoint2f();
+
+    public GroupContoursPipe(TargetGroup group, TargetIntersection intersection) {
+        this.group = group;
+        this.intersection = intersection;
+    }
+
+    public void setConfig(TargetGroup group, TargetIntersection intersection) {
+        this.group = group;
+        this.intersection = intersection;
+    }
+
+    @Override
+    public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<MatOfPoint> input) {
+        long processStartNanos = System.nanoTime();
+
+        groupedContours.forEach(StandardCVPipeline.TrackedTarget::release);
+        groupedContours.clear();
+        contourBuffer.release();
+
+        if (input.size() > (group.equals(TargetGroup.Single) ? 0 : 1)) {
+
+            List<MatOfPoint> sorted = new ArrayList<>(input);
+            sorted.sort(sortByMomentsX);
+
+            Collections.reverse(sorted);
+
+            switch (group) {
+                case Single: {
+                    input.forEach(c -> {
+                        contourBuffer.fromArray(c.toArray());
+                        if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) {
+                            RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
+                            Rect boundingRect = Imgproc.boundingRect(contourBuffer);
+                            var target = new StandardCVPipeline.TrackedTarget();
+                            target.minAreaRect = rect;
+                            target.rawContour = contourBuffer;
+                            target.boundingRect = boundingRect;
+                            groupedContours.add(target);
+                        }
+                    });
+                    break;
+                }
+                case Dual: {
+                    for (var i = 0; i < input.size(); i++) {
+                        List<Point> finalContourList = new ArrayList<>(input.get(i).toList());
+
+                        try {
+                            MatOfPoint firstContour = input.get(i);
+                            MatOfPoint secondContour = input.get(i + 1);
+
+                            if (isIntersecting(firstContour, secondContour)) {
+                                finalContourList.addAll(secondContour.toList());
+                            } else {
+                                finalContourList.clear();
+                                continue;
+                            }
+
+                            intersectMatA.release();
+                            intersectMatB.release();
+
+                            contourBuffer.fromList(finalContourList);
+
+                            if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) {
+                                RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
+                                Rect boundingRect = Imgproc.boundingRect(contourBuffer);
+                                var target = new StandardCVPipeline.TrackedTarget();
+                                target.minAreaRect = rect;
+                                target.boundingRect = boundingRect;
+                                // find left and right bouding rectangles
+                                target.leftRightDualTargetPair =
+                                        Pair.of(Imgproc.boundingRect(firstContour),
+                                                Imgproc.boundingRect(secondContour));
+
+                                // find left and right min area rectangles
+                                tempRectMat.fromArray(firstContour.toArray());
+                                var minAreaRect1 = Imgproc.minAreaRect(tempRectMat);
+                                tempRectMat.fromArray(secondContour.toArray());
+                                var minAreaRect2 = Imgproc.minAreaRect(tempRectMat);
+                                target.leftRightRotatedRect =
+                                        Pair.of(minAreaRect1, minAreaRect2);
+
+                                target.rawContour = contourBuffer;
+
+                                groupedContours.add(target);
+
+                                firstContour.release();
+                                secondContour.release();
+
+                                // skip the next contour because it's been grouped already
+                                i += 1;
+                            }
+                        } catch (IndexOutOfBoundsException e) {
+                            finalContourList.clear();
+                        }
+                    }
+                    break;
+                }
+            }
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(groupedContours, processTime);
+    }
+
+    MatOfPoint2f tempRectMat = new MatOfPoint2f();
+
+    private static double calcMomentsX(MatOfPoint c) {
+        Moments m = Imgproc.moments(c);
+        return (m.get_m10() / m.get_m00());
+    }
+
+    private boolean isIntersecting(MatOfPoint contourOne, MatOfPoint contourTwo) {
+        if (intersection.equals(TargetIntersection.None)) {
+            return true;
+        }
+
+        try {
+            intersectMatA.fromArray(contourOne.toArray());
+            intersectMatB.fromArray(contourTwo.toArray());
+            RotatedRect a = Imgproc.fitEllipse(intersectMatA);
+            RotatedRect b = Imgproc.fitEllipse(intersectMatB);
+            double mA = 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 (intersection) {
+                case Up: {
+                    if (intersectionY < massY) {
+                            return true;
+                    }
+                    break;
+                }
+                case Down: {
+                    if (intersectionY > massY) {
+                            return true;
+                    }
+
+                    break;
+                }
+                case Left: {
+                    if (intersectionX < massX) {
+
+                        return true;
+                    }
+                    break;
+                }
+                case Right: {
+                    if (intersectionX > massX) {
+                        return true;
+                    }
+                    break;
+                }
+            }
+            return false;
+        } catch (Exception e) {
+            return false;
+        }
+    }
+}

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

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

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

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

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

@@ -0,0 +1,55 @@
+package com.chameleonvision._2.vision.pipeline.pipes;
+
+import com.chameleonvision._2.vision.pipeline.Pipe;
+import com.chameleonvision._2.vision.enums.ImageFlipMode;
+import com.chameleonvision._2.vision.enums.ImageRotationMode;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+
+public class RotateFlipPipe implements Pipe<Mat, Mat> {
+
+    private ImageRotationMode rotation;
+    private ImageFlipMode flip;
+
+    private Mat processBuffer = new Mat();
+    private Mat outputMat = new Mat();
+
+    public RotateFlipPipe(ImageRotationMode rotation, ImageFlipMode flip) {
+        this.rotation = rotation;
+        this.flip = flip;
+    }
+
+    public void setConfig(ImageRotationMode rotation, ImageFlipMode flip) {
+        this.rotation = rotation;
+        this.flip = flip;
+    }
+
+    @Override
+    public Pair<Mat, Long> run(Mat input) {
+        long processStartNanos = System.nanoTime();
+
+        boolean shouldFlip = !flip.equals(ImageFlipMode.NONE);
+        boolean shouldRotate = !rotation.equals(ImageRotationMode.DEG_0);
+
+        if (shouldFlip || shouldRotate) {
+//            input.copyTo(processBuffer);
+
+            if (shouldFlip) {
+                Core.flip(input, input, flip.value);
+            }
+
+            if (shouldRotate) {
+                Core.rotate(input, input, rotation.value);
+            }
+
+//            processBuffer.copyTo(outputMat);
+//            processBuffer.release();
+        } else {
+//            input.copyTo(outputMat);
+        }
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(input, processTime);
+    }
+}

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

@@ -0,0 +1,443 @@
+package com.chameleonvision._2.vision.pipeline.pipes;
+
+import com.chameleonvision._2.config.CameraCalibrationConfig;
+import com.chameleonvision._2.vision.pipeline.Pipe;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import org.apache.commons.lang3.tuple.Pair;
+import org.apache.commons.math3.util.FastMath;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.*;
+import org.opencv.imgproc.Imgproc;
+
+import java.util.*;
+import java.util.stream.Collectors;
+
+public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, Mat>, List<StandardCVPipeline.TrackedTarget>> {
+
+    private Double tilt_angle;
+    private MatOfPoint3f objPointsMat = new MatOfPoint3f();
+    private Mat rVec = new Mat();
+    private Mat tVec = new Mat();
+    private Mat rodriguez = new Mat();
+    private Mat pzero_world = new Mat();
+    private Mat cameraMatrix = new Mat();
+    Mat rot_inv = new Mat();
+    Mat kMat = new Mat();
+    private MatOfDouble distortionCoefficients = new MatOfDouble();
+    private List<StandardCVPipeline.TrackedTarget> poseList = new ArrayList<>();
+    Comparator<Point> leftRightComparator = Comparator.comparingDouble(point -> point.x);
+    Comparator<Point> verticalComparator = Comparator.comparingDouble(point -> point.y);
+    private double distanceDivisor = 1.0;
+    Mat scaledTvec = new Mat();
+    MatOfPoint2f boundingBoxResultMat = new MatOfPoint2f();
+    MatOfPoint2f polyOutput = new MatOfPoint2f();
+    private Mat greyImg = new Mat();
+    private double accuracyPercentage = 0.2;
+
+    public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) {
+        super();
+        setCameraCoeffs(calibration);
+//        setBoundingBoxTarget(settings.targetWidth, settings.targetHeight);
+        // TODO add proper year differentiation
+        set2020Target(true);
+
+        this.tilt_angle = tilt.getRadians();
+    }
+
+    public void set2020Target(boolean isHighGoal) {
+        if(isHighGoal) {
+            // tl, bl, br, tr is the order
+            List<Point3> corners = List.of(
+
+                new Point3(-19.625, 0, 0),
+                new Point3(-9.819867, -17, 0),
+                new Point3(9.819867, -17, 0),
+                new Point3(19.625, 0, 0));
+            setObjectCorners(corners);
+        } else {
+            setBoundingBoxTarget(7, 11);
+        }
+    }
+
+    public void setBoundingBoxTarget(double targetWidth, double targetHeight) {
+        // order is left top, left bottom, right bottom, right top
+
+        List<Point3> corners = List.of(
+                new Point3(-targetWidth / 2.0, targetHeight / 2.0, 0.0),
+                new Point3(-targetWidth / 2.0, -targetHeight / 2.0, 0.0),
+                new Point3(targetWidth / 2.0, -targetHeight / 2.0, 0.0),
+                new Point3(targetWidth / 2.0, targetHeight / 2.0, 0.0)
+        );
+        setObjectCorners(corners);
+    }
+
+    public void setObjectCorners(List<Point3> objectCorners) {
+        objPointsMat.release();
+        objPointsMat = new MatOfPoint3f();
+        objPointsMat.fromList(objectCorners);
+    }
+
+    public void setConfig(StandardCVPipelineSettings settings, CameraCalibrationConfig camConfig, Rotation2d tilt) {
+        setCameraCoeffs(camConfig);
+//        setBoundingBoxTarget(settings.targetWidth, settings.targetHeight);
+        // TODO add proper year differentiation
+        tilt_angle = tilt.getRadians();
+        this.objPointsMat = settings.targetCornerMat;
+        this.accuracyPercentage = settings.accuracy.doubleValue();
+    }
+
+    private void setCameraCoeffs(CameraCalibrationConfig settings) {
+        if(settings == null) {
+            System.err.println("SolvePNP can only run on a calibrated resolution, and this one is not! Please calibrate to use solvePNP.");
+            return;
+        }
+        if(cameraMatrix != settings.getCameraMatrixAsMat()) {
+            cameraMatrix.release();
+            settings.getCameraMatrixAsMat().copyTo(cameraMatrix);
+        }
+        if(distortionCoefficients != settings.getDistortionCoeffsAsMat()) {
+            distortionCoefficients.release();
+            settings.getDistortionCoeffsAsMat().copyTo(distortionCoefficients);
+        }
+        this.distanceDivisor = settings.squareSize;
+    }
+
+    @Override
+    public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(Pair<List<StandardCVPipeline.TrackedTarget>, Mat> imageTargetPair) {
+        long processStartNanos = System.nanoTime();
+        var targets = imageTargetPair.getLeft();
+        var image = imageTargetPair.getRight();
+        Imgproc.cvtColor(image, greyImg, Imgproc.COLOR_BGR2GRAY);
+        poseList.clear();
+        for(var target: targets) {
+            MatOfPoint2f corners;
+            if(target.leftRightRotatedRect == null) {
+                corners = find2020VisionTarget(target, accuracyPercentage);//, imageTargetPair.getRight()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target);
+            } else {
+                corners = findCorner2019(target);
+            }
+//            var corners = findCorner2019(target);
+            if(corners == null) continue;
+
+//            // use best features to track
+//            corners = refineCornersByBestTrack(corners, greyImg, target);
+
+            // refine the estimate
+//            corners = refineCornerEstimateSubPix(corners, greyImg);
+
+            var pose = calculatePose(corners, target);
+            if(pose != null) poseList.add(pose);
+        }
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(poseList, processTime);
+    }
+
+    private MatOfPoint2f findCorner2019(StandardCVPipeline.TrackedTarget target) {
+        if(target.leftRightDualTargetPair == null) return null;
+
+        var left = target.leftRightDualTargetPair.getLeft();
+        var right = target.leftRightDualTargetPair.getRight();
+
+        // flip if the "left" target is to the right
+        if(left.x > right.x) {
+            var temp = left;
+            left = right;
+            right = temp;
+        }
+
+        var points = new MatOfPoint2f();
+        points.fromArray(
+                new Point(left.x, left.y + left.height),
+                new Point(left.x, left.y),
+                new Point(right.x + right.width, right.y),
+                new Point(right.x + right.width, right.y + right.height)
+        );
+        return points;
+    }
+
+    MatOfPoint2f target2020ResultMat = new MatOfPoint2f();
+
+    private double distanceBetween(Point a, Point b) {
+        return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2));
+    }
+
+    /**
+     * Find the target using the outermost tape corners and a 2020 target.
+     * @param target the target.
+     * @return The four outermost tape corners.
+     */
+    private MatOfPoint2f find2020VisionTarget(StandardCVPipeline.TrackedTarget target, double accuracyPercentage) {
+        if(target.rawContour.cols() < 1) return null;
+
+        var centroid = target.minAreaRect.center;
+        Comparator<Point> distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
+
+        // algorithm from team 4915
+
+        // Contour perimeter
+        var peri = Imgproc.arcLength(target.rawContour, true);
+        // approximating a shape around the contours
+        // Can be tuned to allow/disallow hulls
+        // Approx is the number of vertices
+        // Ramer–Douglas–Peucker algorithm
+        // we want a number between 0 and 0.16 out of a percentage from 0 to 100
+        // so take accuracy and divide by 600
+        Imgproc.approxPolyDP(target.rawContour, polyOutput, accuracyPercentage / 600.0 * peri, true);
+
+        var area = Imgproc.moments(polyOutput);
+
+//        if (area.get_m00() < 200) {
+//            return null;
+//        }
+
+        var polyList = polyOutput.toList();
+
+        polyOutput.copyTo(target.approxPoly);
+
+        // left top, left bottom, right bottom, right top
+        var boundingBoxCorners = findBoundingBoxCorners(target).toList();
+
+        try {
+
+            // top left and top right are the poly corners closest to the bouding box tl and tr
+            var tl = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get();
+            var tr = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get();
+
+            var bl = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get();
+            var br = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get();
+
+//            polyList = new ArrayList<>(polyList);
+//            polyList.removeAll(List.of(tl, tr, bl, br));
+//
+//            var tl2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get();
+//            var tr2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get();
+//
+//            var bl2 = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get();
+//            var br2 = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get();
+
+            target2020ResultMat.release();
+            target2020ResultMat.fromList(List.of(tl, bl, br, tr));//, tr2, br2, bl2, tl2));
+
+            return target2020ResultMat;
+        } catch (NoSuchElementException e) {
+            return null;
+        }
+    }
+
+    /**
+     * Find the target using the outermost tape corners and a dual target.
+     * @param target the target.
+     * @return The four outermost tape corners.
+     */
+    private MatOfPoint2f findDualTargetCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) {
+        if(target.leftRightRotatedRect == null) return null;
+
+        var centroid = target.minAreaRect.center;
+        Comparator<Point> distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
+
+        var left = target.leftRightRotatedRect.getLeft();
+        var right = target.leftRightRotatedRect.getRight();
+
+        // flip if the "left" target is to the right
+        if(left.center.x > right.center.x) {
+            var temp = left;
+            left = right;
+            right = temp;
+        }
+
+        var leftPoints = new Point[4];
+        left.points(leftPoints);
+        var rightPoints = new Point[4];
+        right.points(rightPoints);
+        ArrayList<Point> combinedList = new ArrayList<>(List.of(leftPoints));
+        combinedList.addAll(List.of(rightPoints));
+
+        // start looking in the top left quadrant
+        var tl = combinedList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get();
+        var tr = combinedList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get();
+        var bl = combinedList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get();
+        var br = combinedList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get();
+
+        boundingBoxResultMat.release();
+        boundingBoxResultMat.fromList(List.of(tl, bl, br, tr));
+
+        return boundingBoxResultMat;
+    }
+
+    /**
+     *
+     * @param target the target to find the corners of.
+     * @return the corners. left top, left bottom, right bottom, right top
+     */
+    private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) {
+
+//        List<Pair<MatOfPoint2f, CVPipeline2d.Target2d>> list = new ArrayList<>();
+//        // find the corners based on the bounding box
+//        // order is left top, left bottom, right bottom, right top
+
+        // extract the corners
+        var points = new Point[4];
+        target.minAreaRect.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);
+
+        boundingBoxResultMat.release();
+        boundingBoxResultMat.fromList(List.of(tl, bl, br, tr));
+
+        return boundingBoxResultMat;
+    }
+
+    MatOfPoint2f goodFeatureToTrackRetval = new MatOfPoint2f();
+
+    private MatOfPoint2f refineCornersByBestTrack(MatOfPoint2f corners, Mat greyImg, StandardCVPipeline.TrackedTarget target) {
+
+        MatOfPoint approxf1 = new MatOfPoint();
+        var origCornerList = new ArrayList<>(corners.toList());
+        approxf1.fromList(origCornerList.stream()
+                .map(it -> new Point(it.x - target.boundingRect.x, it.y - target.boundingRect.y))
+                .collect(Collectors.toList())
+        );
+        var croppedImage = greyImg.submat(target.boundingRect);
+
+        Imgproc.goodFeaturesToTrack(croppedImage, approxf1, 0, 0.1, 5);
+
+        // at this point corners is still unmodified so let's map it
+        List<Point> tempList = new ArrayList<>();
+
+        // shift all points back into global pose
+        var reshiftedList = approxf1.toList().stream().map(it -> new Point(it.x + target.boundingRect.x, it.y + target.boundingRect.y))
+                .collect(Collectors.toList());
+        for(Point p: origCornerList) {
+            // find the goodFeaturesToTrack corner closest to me
+            var closestPoint = reshiftedList.stream().min(Comparator.comparingDouble(p_ -> distanceBetween(p_, p)));
+            if(closestPoint.isEmpty()) {
+                tempList.add(p);
+                reshiftedList.remove(p);
+            } else {
+                tempList.add(closestPoint.get());
+                reshiftedList.remove(closestPoint.get());
+            }
+        }
+
+        goodFeatureToTrackRetval.fromList(tempList);
+        return goodFeatureToTrackRetval;
+    }
+
+    // Set the needed parameters to find the refined corners
+    Size winSize = new Size(4, 4);
+    Size zeroZone = new Size(-1, -1); // we don't need a zero zone
+    TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 90, 0.001);
+
+    private boolean shouldRefineCorners = true;
+
+    /**
+     * Refine an estimated corner position using the cornerSubPixel algorithm.
+     *
+     * TODO should this be here or before the points are chosen?
+     *
+     * @param corners the corners detected -- this mat is modified!
+     * @param greyImg the image taken by the camera as color
+     * @return the updated mat, same as the corner mat passed in.
+     */
+    private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat greyImg) {
+        if(!shouldRefineCorners) return corners; // just return
+        Imgproc.cornerSubPix(greyImg, corners, winSize, zeroZone, criteria);
+
+        return corners;
+    }
+
+//    NetworkTableEntry tvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard").getEntry("tvec");
+//    NetworkTableEntry rvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard").getEntry("rvec");
+
+    public StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) {
+        if(objPointsMat.rows() != imageCornerPoints.rows() || cameraMatrix.rows() < 2 || distortionCoefficients.cols() < 4) {
+            System.err.println("can't do solvePNP with invalid params!");
+            return null;
+        }
+
+        imageCornerPoints.copyTo(target.imageCornerPoints);
+
+        try {
+            Calib3d.solvePnP(objPointsMat, imageCornerPoints, cameraMatrix, distortionCoefficients, rVec, tVec);
+        } catch (Exception e) {
+            e.printStackTrace();
+            return null;
+        }
+
+//        tvecE.setString(tVec.dump());
+//        rvecE.setString(rVec.dump());
+
+        // Algorithm from team 5190 Green Hope Falcons
+
+//        var tilt_angle = 0.0; // TODO add to settings
+
+        // 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
+        var z = tVec.get(2, 0)[0] *  FastMath.cos(tilt_angle) + tVec.get(1, 0)[0] * FastMath.sin(tilt_angle);
+
+        // find skew of the target relative to the camera
+        // from ligerbots:
+        // rot, _ = cv2.Rodrigues(rvec)
+        // rot_inv = rot.transpose()
+        // pzero_world = numpy.matmul(rot_inv, -tvec)
+        // angle2 = math.atan2(pzero_world[0][0], pzero_world[2][0]
+
+        Calib3d.Rodrigues(rVec, rodriguez);
+        Core.transpose(rodriguez, rot_inv); // rodrigurz.t()
+
+        scaledTvec = matScale(tVec, -1);
+        Core.gemm(rot_inv, scaledTvec, 1, kMat, 0, pzero_world);
+
+        var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.get(2, 0)[0]);
+
+        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(z, -x).times(25.4 / 1000d / distanceDivisor);
+        target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation));
+        target.rVector = rVec;
+        target.tVector = tVec;
+
+        return target;
+    }
+
+    /**
+     * 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
+     */
+    public Mat matScale(Mat src, double factor) {
+        Mat dst = new Mat(src.rows(),src.cols(),src.type());
+        Scalar s = new Scalar(factor); // TODO check if we need to add more elements to this
+        Core.multiply(src, s, dst);
+        return dst;
+    }
+
+}

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

@@ -0,0 +1,93 @@
+package com.chameleonvision._2.vision.pipeline.pipes;
+
+import com.chameleonvision._2.vision.camera.CaptureStaticProperties;
+import com.chameleonvision._2.vision.enums.SortMode;
+import com.chameleonvision._2.vision.pipeline.Pipe;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
+import org.apache.commons.lang3.tuple.Pair;
+import org.apache.commons.math3.util.FastMath;
+
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+
+public class SortContoursPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>, List<StandardCVPipeline.TrackedTarget>> {
+
+    private final Comparator<StandardCVPipeline.TrackedTarget> SortByCentermostComparator = Comparator.comparingDouble(this::calcSquareCenterDistance);
+
+    private static final Comparator<StandardCVPipeline.TrackedTarget> SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.minAreaRect.size.area(), rect1.minAreaRect.size.area());
+    private static final Comparator<StandardCVPipeline.TrackedTarget> SortBySmallestComparator = SortByLargestComparator.reversed();
+
+    private static final Comparator<StandardCVPipeline.TrackedTarget> SortByHighestComparator = (rect1, rect2) -> Double.compare(rect1.minAreaRect.center.y, rect2.minAreaRect.center.y);
+    private static final Comparator<StandardCVPipeline.TrackedTarget> SortByLowestComparator = SortByHighestComparator.reversed();
+
+    public static final Comparator<StandardCVPipeline.TrackedTarget> SortByLeftmostComparator = Comparator.comparingDouble(target -> target.minAreaRect.center.x);
+    private static final Comparator<StandardCVPipeline.TrackedTarget> SortByRightmostComparator = SortByLeftmostComparator.reversed();
+
+    private SortMode sort;
+    private CaptureStaticProperties camProps;
+    private int maxTargets;
+
+    private List<StandardCVPipeline.TrackedTarget> sortedContours = new ArrayList<>();
+
+    public SortContoursPipe(SortMode sort, CaptureStaticProperties camProps, int maxTargets) {
+        this.sort = sort;
+        this.camProps = camProps;
+        this.maxTargets = maxTargets;
+    }
+
+    public void setConfig(SortMode sort, CaptureStaticProperties camProps, int maxTargets) {
+        this.sort = sort;
+        this.camProps = camProps;
+        this.maxTargets = maxTargets;
+    }
+
+    @Override
+    public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<StandardCVPipeline.TrackedTarget> input) {
+        long processStartNanos = System.nanoTime();
+
+        sortedContours.clear();
+
+        if (input.size() > 0) {
+            sortedContours.addAll(input);
+
+            switch (sort) {
+                case Largest:
+                    sortedContours.sort(SortByLargestComparator);
+                    break;
+                case Smallest:
+                    sortedContours.sort(SortBySmallestComparator);
+                    break;
+                case Highest:
+                    sortedContours.sort(SortByHighestComparator);
+                    break;
+                case Lowest:
+                    sortedContours.sort(SortByLowestComparator);
+                    break;
+                case Leftmost:
+                    sortedContours.sort(SortByLeftmostComparator);
+                    break;
+                case Rightmost:
+                    sortedContours.sort(SortByRightmostComparator);
+                    break;
+                case Centermost:
+                    sortedContours.sort(SortByCentermostComparator);
+                    break;
+                default:
+                    break;
+            }
+        }
+
+        var sublistedContors = new ArrayList<>(sortedContours.subList(0, Math.min(input.size(), maxTargets - 1)));
+        sortedContours.subList(Math.min(input.size(), maxTargets - 1), sortedContours.size()).forEach(StandardCVPipeline.TrackedTarget::release);
+        sortedContours.clear();
+        sortedContours = new ArrayList<>();
+
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(sublistedContors, processTime);
+    }
+
+    private double calcSquareCenterDistance(StandardCVPipeline.TrackedTarget rect) {
+        return FastMath.sqrt(FastMath.pow(camProps.centerX - rect.minAreaRect.center.x, 2) + FastMath.pow(camProps.centerY - rect.minAreaRect.center.y, 2));
+    }
+}

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

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

chameleon-server/src/main/java/com/chameleonvision/_2/web/RequestHandler.java

@@ -0,0 +1,251 @@
+package com.chameleonvision._2.web;
+
+import com.chameleonvision._2.Main;
+import com.chameleonvision._2.config.ConfigManager;
+import com.chameleonvision._2.network.NetworkManager;
+import com.chameleonvision._2.util.Helpers;
+import com.chameleonvision._2.util.ProgramDirectoryUtilities;
+import com.chameleonvision._2.vision.VisionManager;
+import com.chameleonvision._2.vision.VisionProcess;
+import com.chameleonvision._2.vision.camera.USBCameraCapture;
+import com.chameleonvision._2.vision.pipeline.PipelineManager;
+import com.chameleonvision._2.vision.pipeline.impl.Calibrate3dPipeline;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings;
+import com.chameleonvision.common.datatransfer.networktables.NetworkTablesManager;
+import com.chameleonvision.common.networking.NetworkMode;
+import com.chameleonvision.common.util.Platform;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import io.javalin.http.Context;
+import io.javalin.http.UploadedFile;
+import org.opencv.core.Point3;
+
+import java.io.File;
+import java.io.FileOutputStream;
+import java.io.OutputStream;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+
+public class RequestHandler {
+
+    private static final ObjectMapper kObjectMapper = new ObjectMapper();
+
+    public static void onGeneralSettings(Context ctx) {
+        ObjectMapper objectMapper = kObjectMapper;
+        try {
+            Map map = objectMapper.readValue(ctx.body(), Map.class);
+
+            // TODO: change to function, to restart NetworkTables
+            int newTeamNumber = (int) map.get("teamNumber");
+            if (newTeamNumber != ConfigManager.settings.teamNumber && !NetworkTablesManager.isServer) {
+                NetworkTablesManager.setTeamClientMode();
+            }
+            ConfigManager.settings.teamNumber = newTeamNumber;
+
+            ConfigManager.settings.connectionType = NetworkMode.values()[(int) map.get("connectionType")];
+            ConfigManager.settings.ip = (String) map.get("ip");
+            ConfigManager.settings.netmask = (String) map.get("netmask");
+            ConfigManager.settings.gateway = (String) map.get("gateway");
+            ConfigManager.settings.hostname = (String) map.get("hostname");
+            ConfigManager.saveGeneralSettings();
+            // setting up network config after saving
+            boolean isStatic = ConfigManager.settings.connectionType.equals(NetworkMode.STATIC);
+
+            boolean state = NetworkManager.setHostname(ConfigManager.settings.hostname) && NetworkManager.setNetwork(isStatic, ConfigManager.settings.ip, ConfigManager.settings.netmask, ConfigManager.settings.gateway);
+            if (state) {
+                ctx.status(200);
+            } else {
+                ctx.result("Something went wrong while setting network configuration");
+                ctx.status(501);
+            }
+            SocketHandler.sendFullSettings();
+        } catch (JsonProcessingException e) {
+            ctx.status(500);
+        }
+    }
+
+    public static void onDuplicatePipeline(Context ctx) {
+        ObjectMapper objectMapper = kObjectMapper;
+        try {
+            Map data = objectMapper.readValue(ctx.body(), Map.class);
+
+            int cameraIndex = (Integer) data.getOrDefault("camera", -1);
+
+            var pipelineIndex = (Integer) data.get("pipeline");
+            StandardCVPipelineSettings origPipeline = (StandardCVPipelineSettings) VisionManager.getCurrentUIVisionProcess().pipelineManager.getPipeline(pipelineIndex).settings;
+            String tmp = objectMapper.writeValueAsString(origPipeline);
+            StandardCVPipelineSettings newPipeline = objectMapper.readValue(tmp, StandardCVPipelineSettings.class);
+
+            if (cameraIndex == -1) { // same camera
+
+                VisionManager.getCurrentUIVisionProcess().pipelineManager.duplicatePipeline(newPipeline);
+
+            } else { // another camera
+                var cam = VisionManager.getVisionProcessByIndex(cameraIndex);
+                if (cam != null) {
+                    if (cam.getCamera().getProperties().videoModes.size() < newPipeline.videoModeIndex) {
+                        newPipeline.videoModeIndex = cam.getCamera().getProperties().videoModes.size() - 1;
+                    }
+                    if (newPipeline.is3D) {
+                        var calibration = cam.getCamera().getCalibration(cam.getCamera().getProperties().getVideoMode(newPipeline.videoModeIndex));
+                        if (calibration == null) {
+                            newPipeline.is3D = false;
+                        }
+                    }
+                    VisionManager.getCurrentUIVisionProcess().pipelineManager.duplicatePipeline(newPipeline, cam);
+                    ctx.status(200);
+                } else {
+                    ctx.status(500);
+                }
+            }
+        } catch (JsonProcessingException ex) {
+            ctx.status(500);
+        }
+    }
+
+
+    public static void onCameraSettings(Context ctx) {
+        ObjectMapper objectMapper = kObjectMapper;
+        try {
+            Map camSettings = objectMapper.readValue(ctx.body(), Map.class);
+
+            VisionProcess currentVisionProcess = VisionManager.getCurrentUIVisionProcess();
+            USBCameraCapture currentCamera = currentVisionProcess.getCamera();
+
+            double newFOV, tilt;
+            try {
+                newFOV = (Double) camSettings.get("fov");
+            } catch (Exception ignored) {
+                newFOV = (Integer) camSettings.get("fov");
+            }
+            try {
+                tilt = (Double) camSettings.get("tilt");
+            } catch (Exception ignored) {
+                tilt = (Integer) camSettings.get("tilt");
+            }
+            currentCamera.getProperties().setFOV(newFOV);
+            currentCamera.getProperties().setTilt(Rotation2d.fromDegrees(tilt));
+            VisionManager.saveCurrentCameraSettings();
+            SocketHandler.sendFullSettings();
+            ctx.status(200);
+        } catch (JsonProcessingException e) {
+            e.printStackTrace();
+            ctx.status(500);
+        }
+    }
+
+    public static void onCalibrationStart(Context ctx) throws JsonProcessingException {
+        PipelineManager pipeManager = VisionManager.getCurrentUIVisionProcess().pipelineManager;
+        ObjectMapper objectMapper = kObjectMapper;
+        var data = objectMapper.readValue(ctx.body(), Map.class);
+        int resolutionIndex = (Integer) data.get("resolution");
+        double squareSize;
+        try {
+            squareSize = (Double) data.get("squareSize");
+        } catch (Exception e) {
+            squareSize = (Integer) data.get("squareSize");
+        }
+        // convert from mm to meters
+        pipeManager.calib3dPipe.setSquareSize(squareSize);
+        VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe.settings.videoModeIndex = resolutionIndex;
+        VisionManager.getCurrentUIVisionProcess().pipelineManager.setCalibrationMode(true);
+        VisionManager.getCurrentUIVisionProcess().getCamera().setVideoMode(resolutionIndex);
+    }
+
+    public static void onSnapshot(Context ctx) {
+        Calibrate3dPipeline calPipe = VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe;
+
+        calPipe.takeSnapshot();
+
+        HashMap<String, Object> toSend = new HashMap<>();
+        toSend.put("snapshotCount", calPipe.getSnapshotCount());
+        toSend.put("hasEnough", calPipe.hasEnoughSnapshots());
+
+        ctx.json(toSend);
+        ctx.status(200);
+    }
+
+    public static void onCalibrationEnding(Context ctx) throws JsonProcessingException {
+        PipelineManager pipeManager = VisionManager.getCurrentUIVisionProcess().pipelineManager;
+
+        var data = kObjectMapper.readValue(ctx.body(), Map.class);
+        double squareSize;
+        try {
+            squareSize = (Double) data.get("squareSize");
+        } catch (Exception e) {
+            squareSize = (Integer) data.get("squareSize");
+        }
+        pipeManager.calib3dPipe.setSquareSize(squareSize);
+
+        System.out.println("Finishing Cal");
+        if (pipeManager.calib3dPipe.hasEnoughSnapshots()) {
+            if (pipeManager.calib3dPipe.tryCalibration()) {
+                HashMap<String, Double> tmp = new HashMap<String, Double>();
+                tmp.put("accuracy", pipeManager.calib3dPipe.getCalibrationAccuracy());
+                ctx.json(tmp);
+                ctx.status(200);
+            } else {
+                System.err.println("CALFAIL");
+                ctx.status(500);
+            }
+        } else {
+            ctx.status(201);
+        }
+        pipeManager.setCalibrationMode(false);
+    }
+
+    public static void onPnpModel(Context ctx) throws JsonProcessingException {
+        //noinspection unchecked
+        List<List<Number>> points = kObjectMapper.readValue(ctx.body(), List.class);
+        try {
+            // each entry should be an xy pair
+            var pointsList = new ArrayList<Point3>();
+            for (List<Number> point : points) {
+                double x, y;
+                x = point.get(0).doubleValue();
+                y = point.get(1).doubleValue();
+                var pointToAdd = new Point3(x, y, 0.0);
+                pointsList.add(pointToAdd);
+            }
+            System.out.println(pointsList.toString());
+            if (VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings instanceof StandardCVPipelineSettings) {
+                var settings = (StandardCVPipelineSettings) VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings;
+                settings.targetCornerMat.fromList(pointsList);
+            }
+        } catch (Exception e) {
+            ctx.status(500);
+        }
+    }
+
+    public static void onInstallOrUpdate(Context ctx) {
+        Platform p = Platform.CurrentPlatform;
+        try {
+            if (p == Platform.LINUX_RASPBIAN || p == Platform.LINUX_64) {
+                UploadedFile file = ctx.uploadedFile("file");
+                Path filePath;
+                if (file != null) {
+                    filePath = Paths.get(ProgramDirectoryUtilities.getProgramDirectory(), file.getFilename());
+                    File target = new File(filePath.toString());
+                    OutputStream stream = new FileOutputStream(target);
+                    file.getContent().transferTo(stream);
+                    stream.close();
+                } else {
+                    filePath = Paths.get(new File(Main.class.getProtectionDomain().getCodeSource().getLocation().toURI()).getPath()); // quirk to get the current file directory
+                }
+                Helpers.setService(filePath);
+                ctx.status(200);
+            } else {
+                ctx.result("Only Linux Platforms Support this feature");
+                ctx.status(500);
+            }
+        } catch (Exception e) {
+            ctx.result(e.toString());
+            ctx.status(500);
+        }
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/web/Server.java

@@ -0,0 +1,41 @@
+package com.chameleonvision._2.web;
+
+import com.chameleonvision._2.config.ConfigManager;
+import io.javalin.Javalin;
+
+public class Server {
+    private static SocketHandler socketHandler;
+
+    public static void main(int port) {
+        socketHandler = new SocketHandler();
+
+        Javalin app = Javalin.create(javalinConfig -> {
+            javalinConfig.showJavalinBanner = false;
+            javalinConfig.addStaticFiles("web");
+            javalinConfig.enableCorsForAllOrigins();
+        });
+        app.ws("/websocket", ws -> {
+            ws.onConnect(ctx -> {
+                socketHandler.onConnect(ctx);
+                System.out.println("Socket Connected");
+            });
+            ws.onClose(ctx -> {
+                socketHandler.onClose(ctx);
+                System.out.println("Socket Disconnected");
+                ConfigManager.saveGeneralSettings();
+            });
+            ws.onBinaryMessage(ctx -> {
+                socketHandler.onBinaryMessage(ctx);
+            });
+        });
+        app.post("/api/settings/general", RequestHandler::onGeneralSettings);
+        app.post("/api/settings/camera", RequestHandler::onCameraSettings);
+        app.post("/api/vision/duplicate", RequestHandler::onDuplicatePipeline);
+        app.post("/api/settings/startCalibration", RequestHandler::onCalibrationStart);
+        app.post("/api/settings/snapshot", RequestHandler::onSnapshot);
+        app.post("/api/settings/endCalibration", RequestHandler::onCalibrationEnding);
+        app.post("/api/vision/pnpModel", RequestHandler::onPnpModel);
+        app.post("/api/install", RequestHandler::onInstallOrUpdate);
+        app.start(port);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/_2/web/SocketHandler.java

@@ -0,0 +1,306 @@
+package com.chameleonvision._2.web;
+
+import com.chameleonvision._2.config.CameraCalibrationConfig;
+import com.chameleonvision._2.config.ConfigManager;
+import com.chameleonvision._2.vision.VisionManager;
+import com.chameleonvision._2.vision.VisionProcess;
+import com.chameleonvision._2.vision.camera.CameraCapture;
+import com.chameleonvision._2.vision.camera.CaptureStaticProperties;
+import com.chameleonvision._2.vision.camera.USBCameraCapture;
+import com.chameleonvision._2.vision.enums.ImageRotationMode;
+import com.chameleonvision._2.vision.enums.StreamDivisor;
+import com.chameleonvision._2.vision.pipeline.CVPipeline;
+import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision.common.util.numbers.DoubleCouple;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.core.type.TypeReference;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.cscore.VideoMode;
+import io.javalin.websocket.WsBinaryMessageContext;
+import io.javalin.websocket.WsCloseContext;
+import io.javalin.websocket.WsConnectContext;
+import io.javalin.websocket.WsContext;
+import org.apache.commons.lang3.ArrayUtils;
+import org.msgpack.jackson.dataformat.MessagePackFactory;
+
+import java.lang.reflect.Field;
+import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+
+
+public class SocketHandler {
+
+    private static List<WsContext> users;
+    private static ObjectMapper objectMapper;
+
+    private static final Object broadcastLock = new Object();
+
+    SocketHandler() {
+        users = new ArrayList<>();
+        objectMapper = new ObjectMapper(new MessagePackFactory());
+    }
+
+    void onConnect(WsConnectContext context) {
+        users.add(context);
+        sendFullSettings();
+    }
+
+    void onClose(WsCloseContext context) {
+        users.remove(context);
+    }
+
+    @SuppressWarnings("unchecked")
+    void onBinaryMessage(WsBinaryMessageContext context) throws Exception {
+        Map<String, Object> deserialized = objectMapper.readValue((byte[]) ArrayUtils.toPrimitive(context.data()),
+                new TypeReference<>() {
+                });
+        for (Map.Entry<String, Object> entry : deserialized.entrySet()) {
+            try {
+                VisionProcess currentProcess = VisionManager.getCurrentUIVisionProcess();
+                CameraCapture currentCamera = currentProcess.getCamera();
+                CVPipeline currentPipeline = currentProcess.pipelineManager.getCurrentPipeline();
+//                System.out.println("entry.getKey()+entry.getValue()= " + entry.getKey() + entry.getValue());
+                switch (entry.getKey()) {
+                    case "driverMode": {
+                        HashMap<String, Object> data = (HashMap<String, Object>) entry.getValue();
+                        currentProcess.getDriverModeSettings().exposure = (Integer) data.get("driverExposure");
+                        currentProcess.getDriverModeSettings().brightness = (Integer) data.get("driverBrightness");
+                        currentProcess.setDriverMode((Boolean) data.get("isDriver"));
+
+                        VisionManager.saveCurrentCameraDriverMode();
+                        break;
+                    }
+                    case "changeCameraName": {
+                        currentProcess.setCameraNickname((String) entry.getValue());
+                        sendFullSettings();
+                        VisionManager.saveCurrentCameraSettings();
+                        break;
+                    }
+                    case "changePipelineName": {
+                        currentProcess.pipelineManager.renameCurrentPipeline((String) entry.getValue());
+                        sendFullSettings();
+                        VisionManager.saveCurrentCameraPipelines();
+                        break;
+                    }
+                    case "addNewPipeline": {
+//                        HashMap<String, Object> data = (HashMap<String, Object>) entry.getValue();
+                        String pipeName = (String) entry.getValue();
+                        // TODO: add to UI selection for new 2d/3d
+                        currentProcess.pipelineManager.addNewPipeline(pipeName);
+                        sendFullSettings();
+                        VisionManager.saveCurrentCameraPipelines();
+                        break;
+                    }
+                    case "command": {
+                        switch ((String) entry.getValue()) {
+                            case "deleteCurrentPipeline":
+                                currentProcess.pipelineManager.deleteCurrentPipeline();
+                                sendFullSettings();
+                                VisionManager.saveCurrentCameraPipelines();
+                                break;
+                            case "save":
+                                ConfigManager.saveGeneralSettings();
+                                VisionManager.saveAllCameras();
+                                System.out.println("Saved Settings");
+                                break;
+                        }
+                        // used to define all incoming commands
+                        break;
+                    }
+                    case "currentCamera": {
+                        VisionManager.setCurrentProcessByIndex((Integer) entry.getValue());
+                        sendFullSettings();
+                        break;
+                    }
+                    case "is3D": {
+                        VisionManager.getCurrentUIVisionProcess().setIs3d((Boolean) entry.getValue());
+                        break;
+                    }
+                    case "currentPipeline": {
+                        currentProcess.pipelineManager.setCurrentPipeline((Integer) entry.getValue());
+                        sendFullSettings();
+                        break;
+                    }
+                    case "isPNPCalibration": {
+                        currentProcess.pipelineManager.setCalibrationMode((Boolean) entry.getValue());
+                        break;
+                    }
+                    case "takeCalibrationSnapshot": {
+                        currentProcess.pipelineManager.calib3dPipe.takeSnapshot();
+                    }
+                    default: {
+
+                        switch (entry.getKey()) {//Pre field value set
+                            case "rotationMode": {//Create new CaptureStaticProperties with new width and height, reset crosshair calib
+                                ImageRotationMode oldRot = currentPipeline.settings.rotationMode;
+                                ImageRotationMode newRot = ImageRotationMode.class.getEnumConstants()[(Integer) entry.getValue()];
+                                CaptureStaticProperties prop = currentCamera.getProperties().getStaticProperties();
+                                int width, height;
+                                if (oldRot.isRotated() != newRot.isRotated()) {
+                                    width = prop.mode.height;
+                                    height = prop.mode.width;
+                                    //Creates new video mode with new width and height to create new CaptureStaticProperties and applies it
+                                    currentCamera.getProperties().setStaticProperties(new CaptureStaticProperties(
+                                            new VideoMode(prop.mode.pixelFormat, width, height, prop.mode.fps), prop.fov));
+                                }
+                                prop = currentCamera.getProperties().getStaticProperties();
+                                currentProcess.cameraStreamer.recalculateDivision();
+                                if (currentPipeline instanceof StandardCVPipeline)
+                                    ((StandardCVPipeline) currentPipeline).settings.point.set(prop.mode.width / 2.0, prop.mode.height / 2.0);//Reset Crosshair in single point calib
+                                break;
+                            }
+
+                        }
+
+
+                        if (currentProcess.pipelineManager.getDriverMode()) {
+                            setField(currentProcess.pipelineManager.driverModePipeline.settings, entry.getKey(), entry.getValue());
+                        } else {
+                            setField(currentPipeline.settings, entry.getKey(), entry.getValue());
+                        }
+
+                        //Post field value set
+                        switch (entry.getKey()) {
+                            case "exposure": {
+                                currentCamera.setExposure((Integer) entry.getValue());
+                                break;
+                            }
+                            case "brightness": {
+                                currentCamera.setBrightness((Integer) entry.getValue());
+                                break;
+                            }
+                            case "gain": {
+                                currentCamera.setGain((Integer) entry.getValue());
+                                break;
+                            }
+                            case "videoModeIndex": {
+                                if (currentPipeline instanceof StandardCVPipeline)
+                                    ((StandardCVPipeline) currentPipeline).settings.point = new DoubleCouple();//This will reset the calibration
+                                currentCamera.setVideoMode((Integer) entry.getValue());
+                                currentProcess.cameraStreamer.recalculateDivision();
+                                break;
+                            }
+                            case "streamDivisor": {
+                                currentProcess.cameraStreamer.setDivisor(StreamDivisor.values()[(Integer) entry.getValue()], true);
+                                break;
+                            }
+                        }
+
+                        VisionManager.saveCurrentCameraPipelines();
+                        break;
+                    }
+                }
+            } catch (Exception e) {
+                System.err.println(e.getMessage());
+            }
+            broadcastMessage(deserialized, context);
+        }
+    }
+
+    private void setField(Object obj, String fieldName, Object value) {
+        try {
+            Field field = obj.getClass().getField(fieldName);
+            if (field.getType().isEnum())
+                field.set(obj, field.getType().getEnumConstants()[(Integer) value]);
+            else
+                field.set(obj, value);
+        } catch (NoSuchFieldException | IllegalAccessException ex) {
+            ex.printStackTrace();
+        }
+    }
+
+    private static void broadcastMessage(Object obj, WsContext userToSkip) {
+        synchronized (broadcastLock) {
+            if (users != null) {
+                var userList = users;
+                for (WsContext user : userList) {
+                    if (userToSkip != null && user.getSessionId().equals(userToSkip.getSessionId())) {
+                        continue;
+                    }
+                    try {
+                        ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(obj));
+                        user.send(b);
+                    } catch (JsonProcessingException e) {
+                        e.printStackTrace();
+                    }
+                }
+            }
+        }
+    }
+
+    public static void broadcastMessage(Object obj) {
+        broadcastMessage(obj, null);//Broadcasts the message to every user
+    }
+
+    private static HashMap<String, Object> getOrdinalPipeline(Class cvClass) throws IllegalAccessException {
+        HashMap<String, Object> tmp = new HashMap<>();
+        for (Field field : cvClass.getFields()) { // iterate over every field in CVPipelineSettings
+            try {
+                if (!field.getType().isEnum()) { // if the field is not an enum, get it based on the current pipeline
+                    tmp.put(field.getName(), field.get(VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings));
+                } else {
+                    var ordinal = (Enum) field.get(VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings);
+                    tmp.put(field.getName(), ordinal.ordinal());
+                }
+            } catch (IllegalArgumentException e) {
+                e.printStackTrace();
+            }
+        }
+        return tmp;
+    }
+
+    private static HashMap<String, Object> getOrdinalSettings() {
+        HashMap<String, Object> tmp = new HashMap<>();
+        tmp.put("teamNumber", ConfigManager.settings.teamNumber);
+        tmp.put("connectionType", ConfigManager.settings.connectionType.ordinal());
+        tmp.put("ip", ConfigManager.settings.ip);
+        tmp.put("gateway", ConfigManager.settings.gateway);
+        tmp.put("netmask", ConfigManager.settings.netmask);
+        tmp.put("hostname", ConfigManager.settings.hostname);
+        return tmp;
+    }
+
+    private static HashMap<String, Object> getOrdinalCameraSettings() {
+        HashMap<String, Object> tmp = new HashMap<>();
+        VisionProcess currentVisionProcess = VisionManager.getCurrentUIVisionProcess();
+        USBCameraCapture currentCamera = VisionManager.getCurrentUIVisionProcess().getCamera();
+
+        tmp.put("fov", currentCamera.getProperties().getFOV());
+        tmp.put("streamDivisor", currentVisionProcess.cameraStreamer.getDivisor().ordinal());
+        tmp.put("resolution", currentVisionProcess.getCamera().getProperties().getCurrentVideoModeIndex());
+        tmp.put("tilt", currentVisionProcess.getCamera().getProperties().getTilt().getDegrees());
+
+        List<CameraCalibrationConfig.UICameraCalibrationConfig> calibrations = currentCamera.getAllCalibrationData().stream()
+                .map(CameraCalibrationConfig.UICameraCalibrationConfig::new).collect(Collectors.toList());
+        tmp.put("calibration", calibrations);
+
+        return tmp;
+    }
+
+    public static void sendFullSettings() {
+        //General settings
+        Map<String, Object> fullSettings = new HashMap<>();
+
+        VisionProcess currentProcess = VisionManager.getCurrentUIVisionProcess();
+        CVPipeline currentPipeline = currentProcess.pipelineManager.getCurrentPipeline();
+
+        try {
+            fullSettings.put("settings", getOrdinalSettings());
+            fullSettings.put("cameraSettings", getOrdinalCameraSettings());
+            fullSettings.put("cameraList", VisionManager.getAllCameraNicknames());
+            fullSettings.put("pipeline", getOrdinalPipeline(currentPipeline.settings.getClass()));
+            fullSettings.put("pipelineList", VisionManager.getCurrentCameraPipelineNicknames());
+            fullSettings.put("resolutionList", VisionManager.getCurrentCameraResolutionList());
+            fullSettings.put("port", currentProcess.cameraStreamer.getStreamPort());
+            fullSettings.put("currentPipelineIndex", VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipelineIndex());
+            fullSettings.put("currentCameraIndex", VisionManager.getCurrentUIVisionProcessIndex());
+        } catch (IllegalAccessException e) {
+            System.err.println("No camera found!");
+        }
+        broadcastMessage(fullSettings);
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigManager.java

@@ -1,17 +1,12 @@
 package com.chameleonvision.common.configuration;
 
-import com.chameleonvision.common.server.configuration.MainConfig;
-
 public class ConfigManager {
 
     private final ConfigFolder rootFolder;
-    final MainConfig mainConfig;
 
     protected ConfigManager() {
 
         rootFolder = new ConfigFolder("");
-
-        mainConfig = MainConfig.getInstance();
     }
 
     private static class SingletonHolder {

chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataConsumer.java

@@ -0,0 +1,4 @@
+package com.chameleonvision.common.datatransfer;
+
+public interface DataConsumer {
+}

chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataProvider.java

@@ -0,0 +1,4 @@
+package com.chameleonvision.common.datatransfer;
+
+public interface DataProvider {
+}

chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java

@@ -0,0 +1,75 @@
+package com.chameleonvision.common.datatransfer.networktables;
+
+import com.chameleonvision.common.scripting.ScriptEventType;
+import com.chameleonvision.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 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")) {
+                System.err.println("NT Connection has failed! Will retry in background.");
+                hasReportedConnectionFailure = true;
+            } else if (logMessage.message.contains("connected")) {
+                System.out.println("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;
+        System.out.println("Starting NT Client");
+        ntInstance.stopServer();
+        if (host != null) {
+            ntInstance.startClient(host);
+        } else {
+            ntInstance.startClientTeam(getTeamNumber());
+            if(ntInstance.isConnected()) {
+                System.out.println("[NetworkTablesManager] Connected to the robot!");
+            } else {
+                System.out.println("[NetworkTablesManager] Could NOT to the robot! Will retry in the background...");
+            }
+        }
+    }
+
+    public static void setTeamClientMode() {
+        setClientMode(null);
+    }
+
+    public static void setServerMode() {
+        isServer = true;
+        System.out.println("Starting NT Server");
+        ntInstance.stopClient();
+        ntInstance.startServer();
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/common/logging/DebugLogger.java

@@ -0,0 +1,21 @@
+package com.chameleonvision.common.logging;
+
+public class DebugLogger {
+
+    private final boolean verbose;
+
+    public DebugLogger(boolean verbose) {
+        this.verbose = verbose;
+    }
+
+    public void printInfo(String infoMessage) {
+        if (verbose) {
+            System.out.println(infoMessage);
+        }
+    }
+
+    public void printInfo(String smallInfo, String largeInfo) {
+        System.out.println(verbose ? String.format("%s - %s" , smallInfo, largeInfo) : smallInfo);
+    }
+
+}

chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java

@@ -1,4 +1,4 @@
-package com.chameleonvision.common.network;
+package com.chameleonvision.common.networking;
 
 import org.apache.commons.io.FileUtils;
 

chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkInterface.java

@@ -1,4 +1,4 @@
-package com.chameleonvision.common.network;
+package com.chameleonvision.common.networking;
 
 import java.net.InterfaceAddress;
 

chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkManager.java

@@ -1,4 +1,4 @@
-package com.chameleonvision.common.network;
+package com.chameleonvision.common.networking;
 
 public class NetworkManager {
     private NetworkManager() {}

chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkMode.java

@@ -1,4 +1,4 @@
-package com.chameleonvision.common.network;
+package com.chameleonvision.common.networking;
 
 public enum NetworkMode {
     DHCP,

chameleon-server/src/main/java/com/chameleonvision/common/networking/SysNetworking.java

@@ -1,6 +1,6 @@
-package com.chameleonvision.common.network;
+package com.chameleonvision.common.networking;
 
-import com.chameleonvision.common.server.util.ShellExec;
+import com.chameleonvision.common.util.ShellExec;
 
 import java.io.IOException;
 import java.net.InetAddress;

chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptCommandType.java

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

chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptConfig.java

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

chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java

@@ -0,0 +1,33 @@
+package com.chameleonvision.common.scripting;
+
+
+import com.chameleonvision.common.logging.DebugLogger;
+import com.chameleonvision.common.util.ShellExec;
+
+import java.io.IOException;
+
+public class ScriptEvent {
+    private static final DebugLogger logger = new DebugLogger(true);
+    private static final ShellExec executor = new ShellExec(true, true);
+
+    public final ScriptConfig config;
+
+    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.printInfo(String.format("Output from \"%s\" script: %s\n", config.eventType.name(), output));
+        }
+        logger.printInfo(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;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEventType.java

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

chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java

@@ -0,0 +1,126 @@
+package com.chameleonvision.common.scripting;
+
+import com.chameleonvision.common.logging.DebugLogger;
+import com.chameleonvision.common.util.LoopingRunnable;
+import com.chameleonvision.common.util.Platform;
+import com.chameleonvision.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 DebugLogger logger = new DebugLogger(true);
+
+    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: FIX
+
+        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.printInfo("Queued event: " + eventType.name());
+            } catch (InterruptedException e) {
+                System.err.println("Failed to add event to queue: " + eventType.name());
+            }
+        }
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/common/server/configuration/MainConfig.java

@@ -1,21 +0,0 @@
-package com.chameleonvision.common.server.configuration;
-
-import com.chameleonvision.common.configuration.ConfigFile;
-
-public class MainConfig extends ConfigFile {
-
-    public int teamNumber = 0;
-    public boolean ntServer = false;
-
-    private MainConfig() {
-        super("general");
-    }
-
-    private static class SingletonHolder {
-        private static final MainConfig INSTANCE = new MainConfig();
-    }
-
-    public static MainConfig getInstance() {
-        return SingletonHolder.INSTANCE;
-    }
-}

chameleon-server/src/main/java/com/chameleonvision/common/server/configuration/NetworkConfig.java

@@ -1,23 +0,0 @@
-package com.chameleonvision.common.server.configuration;
-
-import com.chameleonvision.common.configuration.ConfigFile;
-import com.chameleonvision.common.network.NetworkMode;
-
-public class NetworkConfig extends ConfigFile {
-
-    public NetworkMode networkMode = NetworkMode.DHCP;
-    public String ip = "";
-    public String hostname = "chameleon-vision";
-
-    private NetworkConfig() {
-        super("network");
-    }
-
-    private static class SingletonHolder {
-        private static final NetworkConfig INSTANCE = new NetworkConfig();
-    }
-
-    public static NetworkConfig getInstance() {
-        return SingletonHolder.INSTANCE;
-    }
-}

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

@@ -1,45 +0,0 @@
-package com.chameleonvision.common.server.util;
-
-import org.apache.commons.exec.*;
-
-import java.io.ByteArrayOutputStream;
-import java.io.OutputStream;
-
-// TODO: Finish me!
-@SuppressWarnings({"FieldCanBeLocal", "unused"})
-public class ShellExecutor {
-
-    private final Executor executor;
-    private final ExecuteWatchdog watchdog;
-    private final DefaultExecuteResultHandler resultHandler = new DefaultExecuteResultHandler();
-    private final OutputStream stdOutStream = new ByteArrayOutputStream();
-    private final OutputStream stdErrStream = new ByteArrayOutputStream();
-    private final boolean block;
-
-    public ShellExecutor(String command, boolean block, int timeoutMillis, String... args) {
-        this.block = block;
-
-        CommandLine cmdLine = new CommandLine(command);
-        cmdLine.addArguments(args);
-
-        watchdog = new ExecuteWatchdog(timeoutMillis);
-        executor = new DefaultExecutor();
-        executor.setWatchdog(watchdog);
-        executor.setStreamHandler(new PumpStreamHandler(stdOutStream, stdErrStream));
-    }
-
-//    public int execute() {
-//        if ()
-//    }
-
-    public String getStdOut() {
-        if (!watchdog.isWatching()) {
-            return executor.toString();
-        }
-        return "";
-    }
-
-    public String getStdErr() {
-        return "";
-    }
-}

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

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

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

@@ -0,0 +1,37 @@
+package com.chameleonvision.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) {}
+        }
+    }
+}

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

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

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

@@ -1,4 +1,4 @@
-package com.chameleonvision.common.server.util;
+package com.chameleonvision.common.util;
 
 import edu.wpi.first.wpiutil.RuntimeDetector;
 

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

@@ -1,4 +1,4 @@
-package com.chameleonvision.common.server.util;
+package com.chameleonvision.common.util;
 
 import java.io.*;
 

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

@@ -0,0 +1,51 @@
+package com.chameleonvision.common.util.file;
+
+import com.chameleonvision.common.logging.DebugLogger;
+import com.chameleonvision.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 static DebugLogger logger = new DebugLogger(true);
+    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.printInfo("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 {
+            // TODO file perms on Windows
+            System.out.println("File permission setting not available on Windows. Not changing file permissions.");
+        }
+    }
+}

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

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

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

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

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

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

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

@@ -0,0 +1,12 @@
+package com.chameleonvision.common.util.numbers;
+
+public class DoubleCouple extends NumberCouple<Double> {
+
+    public DoubleCouple() {
+        super(0.0, 0.0);
+    }
+
+    public DoubleCouple(Double first, Double second) {
+        super(first, second);
+    }
+}

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

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

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

@@ -0,0 +1,33 @@
+package com.chameleonvision.common.util.numbers;
+
+public class NumberCouple<T extends Number> {
+
+    private T first;
+    private 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;
+    }
+}

chameleon-server/src/main/java/com/chameleonvision/common/vision/base/camera/USBCamera.java

@@ -1,6 +1,6 @@
 package com.chameleonvision.common.vision.base.camera;
 
-import com.chameleonvision.common.vision.base.capture.USBFrameProvider;
+import com.chameleonvision.common.vision.base.frame.provider.USBFrameProvider;
 
 public class USBCamera extends USBFrameProvider {
 }