Pipeline Bringup (#94)

* Refactor package structure, various cleanups

* Add pipeline classes, settings, separate enums

* updated Largest ContourSortMode and added centermost

* Add DriverPipeline classes, apply spotless

* Add crosshair to DriverMode, cleanups

* Add FrameStaticProperties as member in Frame

Add FrameStaticProperties as member in Frame

* Finish ReflectivePipeline, various tweaks

* Apply Spotless

* Move test images

* add Releasable interface, implement in classes

* add TestUtils class, move testimages

* Refactor CVPipeline, add ReflectivePipelineTest

* Fix ConcurrentModificationException bug in group contours pipe with potential targets

* Resolve memory leaks due to unnecessary instantiation of Points

* Apply spotless

* Add CVMat, ReflectionUtils to help track rogue Mats

* various cleanups, add DummyFrameConsumer

* Add logback

* Add slv4j logger to replace the current debugLogger

I'm waiting on stuff to be less skeletoned to add more

* Add perimeter, MatOfPoint2f getters to Contour

* Create CornerDetectionPipe based on old solvePNPPipe

* Add ContourShape class for approxPolyDp Start on ColoredShape tracking

* Add point detection, fix convex hull calculation in Contour

* Make Draw2dContours pipe respect showMultiple

* Update Contour.java

* Clean up draw 3d, fix convex hull bug in corner detection

* Update geometry classes

* Add lifecam calibration data

* Implement solvePNP, bounding box top and bottom

* Fix JSON mat bug and lifecam default calibration for tests, fix 3d drawing

* run spotless

* Refactor calibration into `common.calibration`

* Update .gitignore

* Add offset method to get2020Target

* Various cleanups, add PipelineType enum

* Apply spotless

Co-authored-by: ori agranat <oriagranat9@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>

.gitignore

@@ -122,3 +122,4 @@ New client/chameleon-client/*
 .DS_Store
 # *.iml
 chameleon-server/build
+chameleon-server/chameleon-vision (1).iml

chameleon-server/build.gradle

@@ -68,6 +68,8 @@ dependencies {
     compile "edu.wpi.first.thirdparty.frc2020.opencv:opencv-jni:$openCVVersion:osxx86-64"
     compile "edu.wpi.first.thirdparty.frc2020.opencv:opencv-jni:$openCVVersion:windowsx86-64"
 
+    testCompile "ch.qos.logback:logback-classic:0.9.26"
+
     // javacv (ew)
 //    def withoutJunk = {
 //        exclude group: 'org.bytedeco', module: 'artoolkitplus'
@@ -104,7 +106,6 @@ dependencies {
 sourceSets {
     main {
         java {
-            srcDir 'src'
             exclude '**/_2/**'
         }
     }
@@ -113,9 +114,6 @@ sourceSets {
 test {
     test {
         useJUnitPlatform()
-        testLogging {
-            events "passed", "skipped", "failed"
-        }
     }
 }
 
@@ -125,5 +123,6 @@ spotless {
     paddedCell()
     indentWithTabs(2)
     indentWithSpaces(4)
+    removeUnusedImports()
   }
 }

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

@@ -1,5 +1,7 @@
 package com.chameleonvision._2;
 
+import static com.chameleonvision.common.util.Platform.CurrentPlatform;
+
 import com.chameleonvision._2.config.ConfigManager;
 import com.chameleonvision._2.vision.VisionManager;
 import com.chameleonvision._2.web.Server;
@@ -11,15 +13,14 @@ 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 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";
@@ -44,14 +45,18 @@ public class Main {
                 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("--")) {
+                    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("--")) {
+                    if (potentialPort != null
+                            && !potentialPort.isBlank()
+                            && !potentialPort.startsWith("-") & !potentialPort.startsWith("--")) {
                         value = potentialPort;
                     }
                     i++;
@@ -81,7 +86,8 @@ public class Main {
                             continue;
                         }
                     }
-                    System.err.println("Argument for NT Server Host was invalid, defaulting to team number host");
+                    System.err.println(
+                            "Argument for NT Server Host was invalid, defaulting to team number host");
                     break;
                 case NETWORK_MANAGE_KEY:
                     manageNetwork = false;
@@ -96,7 +102,7 @@ public class Main {
                     if (value != null) {
                         try {
                             uiPort = Integer.parseInt(value);
-                        } catch (NumberFormatException e){
+                        } catch (NumberFormatException e) {
                             System.err.println("ui Port was not a valid number using port 5800");
                         }
                     }
@@ -107,10 +113,13 @@ public class Main {
 
     public static void main(String[] args) {
 
-        Runtime.getRuntime().addShutdownHook(new Thread(() -> ScriptManager.queueEvent(ScriptEventType.kProgramExit)));
+        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());
+            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());
@@ -135,7 +144,8 @@ public class Main {
             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");
+                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!");
         }

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

@@ -8,14 +8,19 @@ 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
- */
+/** 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;
+    @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(
@@ -29,7 +34,8 @@ public class CameraCalibrationConfig {
         this.squareSize = squareSize;
     }
 
-    public CameraCalibrationConfig(Size resolution, Mat cameraMatrix, Mat distortionCoeffs, double squareSize) {
+    public CameraCalibrationConfig(
+            Size resolution, Mat cameraMatrix, Mat distortionCoeffs, double squareSize) {
         this.resolution = resolution;
         this.cameraMatrix = JsonMat.fromMat(cameraMatrix);
         this.distortionCoeffs = JsonMat.fromMat(distortionCoeffs);
@@ -49,16 +55,15 @@ public class CameraCalibrationConfig {
             cameraMatrix = config.cameraMatrix.data;
             distortionCoeffs = config.distortionCoeffs.data;
         }
-
     }
 
     @JsonIgnore
     public Mat getCameraMatrixAsMat() {
-        return cameraMatrix.toMat();
+        return cameraMatrix.getAsMat();
     }
 
     @JsonIgnore
     public MatOfDouble getDistortionCoeffsAsMat() {
-        return new MatOfDouble(distortionCoeffs.toMat());
+        return new MatOfDouble(distortionCoeffs.getAsMat());
     }
 }

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

@@ -1,9 +1,8 @@
 package com.chameleonvision._2.config;
 
+import com.chameleonvision._2.vision.pipeline.CVPipelineSettings;
 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;
@@ -15,7 +14,8 @@ import java.util.Objects;
 
 public class CameraConfig {
 
-    private static final Path camerasConfigFolderPath = Path.of(ConfigManager.SettingsPath.toString(), "cameras");
+    private static final Path camerasConfigFolderPath =
+            Path.of(ConfigManager.SettingsPath.toString(), "cameras");
 
     private final CameraJsonConfig preliminaryConfig;
     private final Path configFolderPath;
@@ -45,7 +45,8 @@ public class CameraConfig {
         checkCalibration();
         pipelineConfig.check();
 
-        return new FullCameraConfiguration(loadConfig(), pipelineConfig.load(), loadDriverMode(), loadCalibration(), this);
+        return new FullCameraConfiguration(
+                loadConfig(), pipelineConfig.load(), loadDriverMode(), loadCalibration(), this);
     }
 
     private CameraJsonConfig loadConfig() {
@@ -53,7 +54,8 @@ public class CameraConfig {
         try {
             config = JacksonUtils.deserialize(configPath, CameraJsonConfig.class);
         } catch (IOException e) {
-            System.err.printf("Failed to load camera config: %s - using default.\n", configPath.toString());
+            System.err.printf(
+                    "Failed to load camera config: %s - using default.\n", configPath.toString());
         }
         return config;
     }
@@ -75,7 +77,10 @@ public class CameraConfig {
     private List<CameraCalibrationConfig> loadCalibration() {
         List<CameraCalibrationConfig> calibrations = new ArrayList<>();
         try {
-            calibrations = List.of(Objects.requireNonNull(JacksonUtils.deserialize(calibrationPath, CameraCalibrationConfig[].class)));
+            calibrations =
+                    List.of(
+                            Objects.requireNonNull(
+                                    JacksonUtils.deserialize(calibrationPath, CameraCalibrationConfig[].class)));
         } catch (Exception e) {
             System.err.println("Failed to load camera calibration: " + driverModePath.toString());
         }
@@ -104,7 +109,6 @@ public class CameraConfig {
         }
     }
 
-
     public void saveCalibration(List<CameraCalibrationConfig> cal) {
         CameraCalibrationConfig[] configs = cal.toArray(new CameraCalibrationConfig[0]);
         try {
@@ -119,7 +123,8 @@ public class CameraConfig {
         if (!configFolderExists()) {
             try {
                 if (!(new File(configFolderPath.toUri()).mkdirs())) {
-                    System.err.println("Failed to create camera config folder: " + configFolderPath.toString());
+                    System.err.println(
+                            "Failed to create camera config folder: " + configFolderPath.toString());
                 }
                 FileUtils.setFilePerms(configFolderPath);
             } catch (Exception e) {
@@ -158,7 +163,8 @@ public class CameraConfig {
                 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());
+                System.err.println(
+                        "Failed to create camera calibration file: " + calibrationPath.toString());
             }
         }
     }

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

@@ -48,6 +48,13 @@ public class CameraJsonConfig {
         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);
+        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

@@ -6,7 +6,6 @@ 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;
@@ -17,10 +16,10 @@ import java.util.LinkedHashMap;
 import java.util.List;
 
 public class ConfigManager {
-    private ConfigManager() {
-    }
+    private ConfigManager() {}
 
-    public static final Path SettingsPath = Paths.get(ProgramDirectoryUtilities.getProgramDirectory(), "settings");
+    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<>();
@@ -46,14 +45,14 @@ public class ConfigManager {
                     new ShellExec().executeBashCommand("sudo chmod -R 0777 " + SettingsPath.toString());
                 }
             } catch (IOException e) {
-                if (!(e instanceof java.nio.file.FileAlreadyExistsException))
-                    e.printStackTrace();
+                if (!(e instanceof java.nio.file.FileAlreadyExistsException)) e.printStackTrace();
             }
         }
     }
 
     private static void checkSettingsFile() {
-        boolean settingsFileEmpty = settingsFileExists() && new File(settingsFilePath.toString()).length() == 0;
+        boolean settingsFileEmpty =
+                settingsFileExists() && new File(settingsFilePath.toString()).length() == 0;
         if (settingsFileEmpty || !settingsFileExists()) {
             try {
                 JacksonUtils.serializer(settingsFilePath, settings, true);
@@ -91,7 +90,8 @@ public class ConfigManager {
         saveSettingsFile();
     }
 
-    public static List<FullCameraConfiguration> initializeCameras(List<CameraJsonConfig> preliminaryConfigs) {
+    public static List<FullCameraConfiguration> initializeCameras(
+            List<CameraJsonConfig> preliminaryConfigs) {
         List<FullCameraConfiguration> configList = new ArrayList<>();
 
         checkSettingsFolder();

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

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

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

@@ -3,12 +3,12 @@ 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;
+    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

@@ -1,18 +1,22 @@
 package com.chameleonvision._2.config;
 
+import com.chameleonvision.common.vision.opencv.Releasable;
+import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import java.util.Arrays;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
 
-import java.util.Arrays;
-
-
-public class JsonMat {
+public class JsonMat implements Releasable {
     public final int rows;
     public final int cols;
     public final int type;
     public final double[] data;
 
+    @JsonIgnore private Mat wrappedMat;
+    private MatOfDouble wrappedMatOfDouble;
+
     public JsonMat(int rows, int cols, double[] data) {
         this(rows, cols, CvType.CV_64FC1, data);
     }
@@ -28,10 +32,6 @@ public class JsonMat {
         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;
     }
@@ -44,10 +44,11 @@ public class JsonMat {
         return isDistortionCoeffsMat(mat) || isCameraMatrixMat(mat);
     }
 
+    @JsonIgnore
     public static double[] getDataFromMat(Mat mat) {
         if (!isCalibrationMat(mat)) return null;
 
-        double[] data = new double[(int)(mat.total()*mat.elemSize())];
+        double[] data = new double[(int) (mat.total() * mat.elemSize())];
         mat.get(0, 0, data);
 
         int dataLen = -1;
@@ -64,11 +65,28 @@ public class JsonMat {
         return new JsonMat(mat.rows(), mat.cols(), getDataFromMat(mat));
     }
 
-    public static Mat toMat(JsonMat jsonMat) {
-        if (jsonMat.type != CvType.CV_64FC1) return null;
+    @JsonIgnore
+    public Mat getAsMat() {
+        if (this.type != CvType.CV_64FC1) return null;
 
-        Mat retMat = new Mat(jsonMat.rows, jsonMat.cols, jsonMat.type);
-        retMat.put(0, 0, jsonMat.data);
-        return retMat;
+        if (wrappedMat == null) {
+            this.wrappedMat = new Mat(this.rows, this.cols, this.type);
+            this.wrappedMat.put(0, 0, this.data);
+        }
+        return this.wrappedMat;
+    }
+
+    @JsonIgnore
+    public MatOfDouble getAsMatOfDouble() {
+        if (this.wrappedMatOfDouble == null) {
+            this.wrappedMatOfDouble = new MatOfDouble();
+            getAsMat().convertTo(wrappedMatOfDouble, CvType.CV_64F);
+        }
+        return this.wrappedMatOfDouble;
+    }
+
+    @Override
+    public void release() {
+        getAsMat().release();
     }
 }

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

@@ -6,7 +6,6 @@ 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;
@@ -20,10 +19,10 @@ public class PipelineConfig {
     private final CameraConfig cameraConfig;
 
     /**
-     * Construct a new PipelineConfig
-     *
-     * @param cameraConfig the CameraConfig (parent folder, kinda?)
-     */
+    * Construct a new PipelineConfig
+    *
+    * @param cameraConfig the CameraConfig (parent folder, kinda?)
+    */
     PipelineConfig(CameraConfig cameraConfig) {
         this.cameraConfig = cameraConfig;
     }
@@ -76,7 +75,12 @@ public class PipelineConfig {
 
         if (settings instanceof StandardCVPipelineSettings) {
             try {
-                JacksonUtils.serialize(path, (StandardCVPipelineSettings) settings, StandardCVPipelineSettings.class, new StandardCVPipelineSettingsSerializer(), true);
+                JacksonUtils.serialize(
+                        path,
+                        (StandardCVPipelineSettings) settings,
+                        StandardCVPipelineSettings.class,
+                        new StandardCVPipelineSettingsSerializer(),
+                        true);
                 FileUtils.setFilePerms(path);
             } catch (IOException e) {
                 e.printStackTrace();
@@ -131,7 +135,11 @@ public class PipelineConfig {
         } else {
             for (File pipelineFile : pipelineFiles) {
                 try {
-                    var pipe = JacksonUtils.deserialize(Paths.get(pipelineFile.getPath()), StandardCVPipelineSettings.class, new StandardCVPipelineSettingsDeserializer());
+                    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");

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

@@ -8,12 +8,11 @@ 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;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point3;
 
 public abstract class BaseDeserializer<T> extends StdDeserializer<T> {
     protected BaseDeserializer(Class<?> vc) {
@@ -22,14 +21,18 @@ public abstract class BaseDeserializer<T> extends StdDeserializer<T> {
 
     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 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 {
+    IntegerCouple getNumberCouple(String name, IntegerCouple defaultValue)
+            throws JsonProcessingException {
         JsonNode node = baseNode.get(name);
 
         if (nodeGood(node)) {
@@ -39,7 +42,8 @@ public abstract class BaseDeserializer<T> extends StdDeserializer<T> {
         return defaultValue;
     }
 
-    DoubleCouple getNumberCouple(String name, DoubleCouple defaultValue) throws JsonProcessingException {
+    DoubleCouple getNumberCouple(String name, DoubleCouple defaultValue)
+            throws JsonProcessingException {
         JsonNode node = baseNode.get(name);
 
         if (nodeGood(node)) {
@@ -49,7 +53,8 @@ public abstract class BaseDeserializer<T> extends StdDeserializer<T> {
         return defaultValue;
     }
 
-    List<Number> getNumberList(String name, List<Number> defaultValue) throws JsonProcessingException {
+    List<Number> getNumberList(String name, List<Number> defaultValue)
+            throws JsonProcessingException {
         JsonNode node = baseNode.get(name);
 
         if (nodeGood(node)) {
@@ -69,7 +74,7 @@ public abstract class BaseDeserializer<T> extends StdDeserializer<T> {
     }
 
     int getInt(String name, int defaultValue) {
-        return (int)  getDouble(name, defaultValue);
+        return (int) getDouble(name, defaultValue);
     }
 
     double getDouble(String name, double defaultValue) {
@@ -92,7 +97,8 @@ public abstract class BaseDeserializer<T> extends StdDeserializer<T> {
         return defaultValue;
     }
 
-    <E extends Enum<E>> E getEnum(String name, Class<E> enumClass, E defaultValue) throws IOException {
+    <E extends Enum<E>> E getEnum(String name, Class<E> enumClass, E defaultValue)
+            throws IOException {
         JsonNode node = baseNode.get(name);
 
         if (nodeGood(node)) {
@@ -108,12 +114,14 @@ public abstract class BaseDeserializer<T> extends StdDeserializer<T> {
 
         return defaultValue;
     }
-    MatOfPoint3f getMatOfPoint3f(String name, MatOfPoint3f defaultValue) throws JsonProcessingException {
+
+    MatOfPoint3f getMatOfPoint3f(String name, MatOfPoint3f defaultValue)
+            throws JsonProcessingException {
         JsonNode node = baseNode.get(name);
-        if (nodeGood(node)){
+        if (nodeGood(node)) {
             List<List<Number>> numberList = mapper.readValue(node.toString(), pointListColType);
             List<Point3> point3List = new ArrayList<>();
-            for (List<Number> tmp : numberList){
+            for (List<Number> tmp : numberList) {
                 Point3 p = new Point3();
                 p.x = tmp.get(0).doubleValue();
                 p.y = tmp.get(1).doubleValue();

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

@@ -3,11 +3,10 @@ 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;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point3;
 
 public abstract class BaseSerializer<T> extends StdSerializer<T> {
     protected BaseSerializer(Class<T> t) {
@@ -16,7 +15,8 @@ public abstract class BaseSerializer<T> extends StdSerializer<T> {
 
     JsonGenerator generator;
 
-    <N extends NumberCouple> void writeNumberCoupleAsNumberArray(String name, N couple) throws IOException {
+    <N extends NumberCouple> void writeNumberCoupleAsNumberArray(String name, N couple)
+            throws IOException {
         generator.writeArrayFieldStart(name);
         generator.writeObject(couple.getFirst());
         generator.writeObject(couple.getSecond());

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

@@ -5,10 +5,10 @@ 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 class StandardCVPipelineSettingsDeserializer
+        extends BaseDeserializer<StandardCVPipelineSettings> {
     public StandardCVPipelineSettingsDeserializer() {
         this(null);
     }
@@ -18,7 +18,8 @@ public class StandardCVPipelineSettingsDeserializer extends BaseDeserializer<Sta
     }
 
     @Override
-    public StandardCVPipelineSettings deserialize(JsonParser jsonParser, DeserializationContext ctxt) throws IOException, JsonProcessingException {
+    public StandardCVPipelineSettings deserialize(JsonParser jsonParser, DeserializationContext ctxt)
+            throws IOException, JsonProcessingException {
         // set BaseDeserializer parser reference.
         baseNode = jsonParser.getCodec().readTree(jsonParser);
 
@@ -56,19 +57,24 @@ public class StandardCVPipelineSettingsDeserializer extends BaseDeserializer<Sta
 
         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.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.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.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);

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

@@ -3,10 +3,10 @@ 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 class StandardCVPipelineSettingsSerializer
+        extends BaseSerializer<StandardCVPipelineSettings> {
     public StandardCVPipelineSettingsSerializer() {
         this(null);
     }
@@ -16,7 +16,9 @@ public class StandardCVPipelineSettingsSerializer extends BaseSerializer<Standar
     }
 
     @Override
-    public void serialize(StandardCVPipelineSettings pipeline, JsonGenerator gen, SerializerProvider provider) throws IOException {
+    public void serialize(
+            StandardCVPipelineSettings pipeline, JsonGenerator gen, SerializerProvider provider)
+            throws IOException {
         // set BaseSerializer generator reference.
         generator = gen;
 

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

@@ -1,7 +1,5 @@
 package com.chameleonvision._2.network;
 
-import org.apache.commons.io.FileUtils;
-
 import java.io.File;
 import java.io.IOException;
 import java.net.InetAddress;
@@ -10,6 +8,7 @@ import java.nio.charset.StandardCharsets;
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
+import org.apache.commons.io.FileUtils;
 
 public class LinuxNetworking extends SysNetworking {
     private static final String PATH = "/etc/dhcpcd.conf";
@@ -26,7 +25,8 @@ public class LinuxNetworking extends SysNetworking {
                         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")) {
+                            if (subInterface.contains("static ip_address")
+                                    || subInterface.contains("static routers")) {
                                 lines.remove(j);
                                 j--;
                             }
@@ -81,7 +81,6 @@ public class LinuxNetworking extends SysNetworking {
         return false;
     }
 
-
     @Override
     public List<java.net.NetworkInterface> getNetworkInterfaces() throws SocketException {
         List<java.net.NetworkInterface> netInterfaces;
@@ -98,6 +97,5 @@ public class LinuxNetworking extends SysNetworking {
             goodInterfaces.add(netInterface);
         }
         return goodInterfaces;
-
     }
 }

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

@@ -3,7 +3,8 @@ 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
+    // 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();

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

@@ -4,19 +4,19 @@ 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 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();
+    public NetworkInterface(java.net.NetworkInterface inetface, InterfaceAddress ifaceAddress) {
+        name = inetface.getName();
+        displayName = inetface.getDisplayName();
 
-		var inetAddress = ifaceAddress.getAddress();
-		IPAddress = inetAddress.getHostAddress();
+        var inetAddress = ifaceAddress.getAddress();
+        IPAddress = inetAddress.getHostAddress();
         Netmask = getIPv4LocalNetMask(ifaceAddress);
 
         // TODO: (low) hack to "get" gateway, this is gross and bad, pls fix
@@ -25,14 +25,14 @@ public class NetworkInterface {
         Gateway = String.join(".", splitIPAddr);
         splitIPAddr[3] = "255";
         Broadcast = String.join(".", splitIPAddr);
-	}
+    }
 
     private static String getIPv4LocalNetMask(InterfaceAddress interfaceAddress) {
-	    var netPrefix = interfaceAddress.getNetworkPrefixLength();
+        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);
+            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...
@@ -41,11 +41,16 @@ public class NetworkInterface {
             }
             // 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) {
+            // 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...

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

@@ -2,14 +2,12 @@ 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 NetworkManager() {}
 
     private static SysNetworking networking;
     private static boolean isManaged = false;
@@ -25,7 +23,7 @@ public class NetworkManager {
         if (platform.isLinux()) {
             networking = new LinuxNetworking();
         } else if (platform.isWindows()) {
-//			networking = new WindowsNetworking();
+            //			networking = new WindowsNetworking();
             System.out.println("Windows networking is not yet supported. Running unmanaged.");
             return;
         }
@@ -71,10 +69,9 @@ public class NetworkManager {
     }
 
     private static byte[] GetTeamNumberIPBytes(int teamNumber) {
-        return new byte[]{(byte) (teamNumber / 100), (byte) (teamNumber % 100)};
+        return new byte[] {(byte) (teamNumber / 100), (byte) (teamNumber % 100)};
     }
 
-
     private static boolean setDHCP() {
         if (!isManaged) {
             return true;

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

@@ -1,36 +1,38 @@
 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);
+    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 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;
+    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

@@ -7,48 +7,51 @@ import java.util.List;
 
 public class WindowsNetworking extends SysNetworking {
 
-	@Override
-	public boolean setDHCP() {
-		return false;
-	}
+    @Override
+    public boolean setDHCP() {
+        return false;
+    }
 
-	@Override
-	public boolean setHostname(String newHostname) {
-		var currentHostname = getHostname();
+    @Override
+    public boolean setHostname(String newHostname) {
+        var currentHostname = getHostname();
 
-		if (getHostname() == null) {
-			return false;
-		}
+        if (getHostname() == null) {
+            return false;
+        }
 
-		String command = String.format("wmic computersystem where name=\"%s\" call rename name=\"%s\"", currentHostname, newHostname);
+        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;
-		}
-	}
+        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 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());
+    @Override
+    public List<java.net.NetworkInterface> getNetworkInterfaces() throws SocketException {
+        var netInterfaces = Collections.list(java.net.NetworkInterface.getNetworkInterfaces());
 
-		List<java.net.NetworkInterface> goodInterfaces = new ArrayList<>();
+        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;
-	}
+        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

@@ -1,7 +1,6 @@
 package com.chameleonvision._2.util;
 
 import edu.wpi.cscore.VideoMode;
-
 import java.io.File;
 import java.io.FileWriter;
 import java.io.IOException;
@@ -13,31 +12,32 @@ 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());
-        }};
+        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";
-
+    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());

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

@@ -5,10 +5,12 @@ import java.net.URISyntaxException;
 
 public class ProgramDirectoryUtilities {
     private static String getJarName() {
-        return new File(ProgramDirectoryUtilities.class.getProtectionDomain()
-                .getCodeSource()
-                .getLocation()
-                .getPath())
+        return new File(
+                        ProgramDirectoryUtilities.class
+                                .getProtectionDomain()
+                                .getCodeSource()
+                                .getLocation()
+                                .getPath())
                 .getName();
     }
 
@@ -27,11 +29,18 @@ public class ProgramDirectoryUtilities {
 
     private static String getCurrentJARDirectory() {
         try {
-            return new File(ProgramDirectoryUtilities.class.getProtectionDomain().getCodeSource().getLocation().toURI().getPath()).getParent();
+            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

@@ -8,17 +8,17 @@ 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;
+import org.opencv.videoio.VideoCapture;
 
 @SuppressWarnings("rawtypes")
 public class VisionManager {
     private VisionManager() {}
 
-    private static final LinkedHashMap<String, UsbCameraInfo> usbCameraInfosByCameraName = new LinkedHashMap<>();
+    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<>();
 
@@ -43,7 +43,8 @@ public class VisionManager {
             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
+                // 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++;
@@ -61,17 +62,22 @@ public class VisionManager {
         // load the config
         List<CameraJsonConfig> preliminaryConfigs = new ArrayList<>();
 
-        usbCameraInfosByCameraName.forEach((suffixedName, cameraInfo) -> {
-            String truePath;
+        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);
-            }
+                    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));
-        });
+                    preliminaryConfigs.add(new CameraJsonConfig(truePath, suffixedName));
+                });
 
         loadedCameraConfigs.addAll(ConfigManager.initializeCameras(preliminaryConfigs));
         System.out.printf("[VisionManager] Loaded %s cameras!\n", loadedCameraConfigs.size());
@@ -92,7 +98,9 @@ public class VisionManager {
         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);
+        System.out.printf(
+                "[VisionManager] Loaded %s vision processes! Current process: %s\n",
+                visionProcesses.size(), visionProcesses.get(0).name);
         return true;
     }
 
@@ -110,7 +118,12 @@ public class VisionManager {
 
     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;
+        return Objects.requireNonNull(
+                        loadedCameraConfigs.stream()
+                                .filter(x -> x.cameraConfig.name.equals(cameraName))
+                                .findFirst()
+                                .orElse(null))
+                .fileConfig;
     }
 
     public static void setCurrentProcessByIndex(int processIndex) {
@@ -127,32 +140,42 @@ public class VisionManager {
             return null;
         }
 
-        VisionProcessManageable vpm =  visionProcesses.stream().filter(manageable -> manageable.index == processIndex).findFirst().orElse(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());
+        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());
+        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);
-        });
+        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() {
@@ -173,7 +196,9 @@ public class VisionManager {
     }
 
     private static List<HashMap> getCameraResolutionList(USBCameraCapture capture) {
-        return capture.getProperties().getVideoModes().stream().map(Helpers::VideoModeToHashMap).collect(Collectors.toList());
+        return capture.getProperties().getVideoModes().stream()
+                .map(Helpers::VideoModeToHashMap)
+                .collect(Collectors.toList());
     }
 
     public static List<HashMap> getCurrentCameraResolutionList() {
@@ -181,7 +206,11 @@ public class VisionManager {
     }
 
     public static int getCurrentUIVisionProcessIndex() {
-        VisionProcessManageable vpm = visionProcesses.stream().filter(v -> v.visionProcess == currentUIVisionProcess).findFirst().orElse(null);
+        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

@@ -23,14 +23,12 @@ 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;
-
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
 
 @SuppressWarnings("rawtypes")
 public class VisionProcess {
@@ -75,31 +73,38 @@ public class VisionProcess {
         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);
+        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());
+        defaultTable =
+                NetworkTableInstance.getDefault()
+                        .getTable("/chameleon-vision/" + cameraCapture.getProperties().getNickname());
     }
 
     public void start() {
-        System.out.printf("[%s Process] Creating network table...\n", getCamera().getProperties().getNickname());
+        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());
+        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}
-     */
+    * 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);
@@ -128,8 +133,10 @@ public class VisionProcess {
         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);
+        ntDriveModeListenerID =
+                ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate);
+        ntPipelineListenerID =
+                ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate);
         ntDriverModeEntry.setBoolean(false);
         ntPipelineEntry.setNumber(pipelineManager.getCurrentPipelineIndex());
         pipelineManager.ntIndexEntry = ntPipelineEntry;
@@ -141,15 +148,16 @@ public class VisionProcess {
 
     public void setDriverMode(boolean driverMode) {
         pipelineManager.setDriverMode(driverMode);
-        ScriptManager.queueEvent(driverMode ? ScriptEventType.kEnterDriverMode : ScriptEventType.kExitDriverMode);
+        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
-     */
+    * 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) {
@@ -173,7 +181,6 @@ public class VisionProcess {
         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<>();
@@ -181,13 +188,14 @@ public class VisionProcess {
                 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.StandardCVPipelineResult result =
+                                (StandardCVPipeline.StandardCVPipelineResult) data;
                         StandardCVPipeline.TrackedTarget bestTarget = result.targets.get(0);
                         try {
-                            if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings).multiple) {
+                            if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings)
+                                    .multiple) {
                                 for (var target : result.targets) {
                                     pointMap = new HashMap<>();
                                     pointMap.put("pitch", target.pitch);
@@ -206,7 +214,7 @@ public class VisionProcess {
                             center.add(bestTarget.minAreaRect.center.x);
                             center.add(bestTarget.minAreaRect.center.y);
                         } catch (ClassCastException ignored) {
-                          
+
                         }
                     } else {
                         pointMap.put("pitch", null);
@@ -236,7 +244,8 @@ public class VisionProcess {
             if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
 
                 //noinspection unchecked
-                List<StandardCVPipeline.TrackedTarget> targets = (List<StandardCVPipeline.TrackedTarget>) data.targets;
+                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);
@@ -249,12 +258,30 @@ public class VisionProcess {
                 ntTargetRotation.setDouble(bestTarget.minAreaRect.angle);
                 try {
                     Pose2d targetPose = targets.get(0).cameraRelativePose;
-                    double[] targetArray = {targetPose.getTranslation().getX(), targetPose.getTranslation().getY(), targetPose.getRotation().getDegrees()};
+                    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())));
+                    //
+                    // 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();
                 }
@@ -311,9 +338,7 @@ public class VisionProcess {
         return false;
     }
 
-    /**
-     * VisionProcessRunnable will process images as quickly as possible
-     */
+    /** VisionProcessRunnable will process images as quickly as possible */
     private class VisionProcessRunnable implements Runnable {
 
         volatile Double fps = 0.0;
@@ -324,21 +349,23 @@ public class VisionProcess {
             var lastUpdateTimeNanos = System.nanoTime();
             var lastStreamTimeMs = System.currentTimeMillis();
 
-            System.out.printf("[%s Process] Vision Process Thread -- first run!\n", getCamera().getProperties().getNickname());
+            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() + "!");
+                        System.err.println(
+                                "Exception in vision process " + getCamera().getProperties().getNickname() + "!");
                         e.printStackTrace();
                     }
 
@@ -355,18 +382,22 @@ public class VisionProcess {
                 try {
                     var currentTime = System.currentTimeMillis();
                     if ((currentTime - lastStreamTimeMs) / 1000d > 1.0 / 30.0) {
-                        if(lastPipelineResult != null) {
+                        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());
+                            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());
+                    //                    Debug.printInfo("Vision running faster than stream.");
+                    System.err.printf(
+                            "[%s Process] Exception in vision thread!\n",
+                            getCamera().getProperties().getNickname());
                     e.printStackTrace();
                 }
 

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

@@ -4,7 +4,6 @@ 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 {
@@ -13,36 +12,41 @@ public interface CameraCapture extends ImageCapture {
     VideoMode getCurrentVideoMode();
 
     /**
-     * Set the exposure of the camera
-     * @param exposure the new exposure to set the camera to
-     */
+    * 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
-     */
+    * 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
-     */
+    * 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
-     */
+    * 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
-     */
+    * 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

@@ -24,14 +24,17 @@ public class CameraStreamer {
         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);
+        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
-        );
+        this.size =
+                new Size(
+                        cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value,
+                        cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value);
         setDivisor(divisor, false);
     }
 
@@ -44,15 +47,16 @@ public class CameraStreamer {
         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));
+            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() {
@@ -80,21 +84,24 @@ public class CameraStreamer {
         }
 
         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 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) {
+        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.setVideoMode(
+                        new VideoMode(
+                                sourceVideoMode.pixelFormat,
+                                (int) imageSize.width,
+                                (int) imageSize.height,
+                                sourceVideoMode.fps));
             }
         }
 

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

@@ -31,9 +31,11 @@ public class CaptureStaticProperties {
         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));
+        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

@@ -8,13 +8,12 @@ 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;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
 
 public class USBCameraCapture implements CameraCapture {
     private final UsbCamera baseCamera;
@@ -25,22 +24,26 @@ public class USBCameraCapture implements CameraCapture {
 
     public USBCameraCapture(FullCameraConfiguration fullCameraConfiguration) {
         var config = fullCameraConfiguration.cameraConfig;
-        this.calibrationList = new ArrayList<>(); //fullCameraConfiguration.calibration;
+        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.");
+        } 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()  );
+        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;
@@ -48,8 +51,8 @@ public class USBCameraCapture implements CameraCapture {
     }
 
     public CameraCalibrationConfig getCalibration(Size size) {
-        for(var calibration: calibrationList) {
-            if(calibration.resolution.equals(size)) return calibration;
+        for (var calibration : calibrationList) {
+            if (calibration.resolution.equals(size)) return calibration;
         }
         return null;
     }
@@ -59,7 +62,10 @@ public class USBCameraCapture implements CameraCapture {
     }
 
     public void addCalibrationData(CameraCalibrationConfig newConfig) {
-        calibrationList.removeIf(c -> newConfig.resolution.height == c.resolution.height && newConfig.resolution.width == c.resolution.width);
+        calibrationList.removeIf(
+                c ->
+                        newConfig.resolution.height == c.resolution.height
+                                && newConfig.resolution.width == c.resolution.width);
         calibrationList.add(newConfig);
     }
 
@@ -79,7 +85,8 @@ public class USBCameraCapture implements CameraCapture {
         // 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 =
+        // 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);
@@ -113,7 +120,7 @@ public class USBCameraCapture implements CameraCapture {
         }
     }
 
-    public void setVideoMode(int index){
+    public void setVideoMode(int index) {
         VideoMode mode = properties.getVideoModes().get(index);
         setVideoMode(mode);
     }

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

@@ -6,7 +6,6 @@ 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;
@@ -25,11 +24,16 @@ public class USBCaptureProperties extends CaptureProperties {
     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 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));
+    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;
@@ -89,7 +93,8 @@ public class USBCaptureProperties extends CaptureProperties {
     }
 
     private List<VideoMode> filterVideoModes(VideoMode[] videoModes) {
-        Predicate<VideoMode> fullPredicate = kMinFPSPredicate.and(kMinSizePredicate).and(kPixelFormatPredicate);
+        Predicate<VideoMode> fullPredicate =
+                kMinFPSPredicate.and(kMinSizePredicate).and(kPixelFormatPredicate);
         Stream<VideoMode> validModes = Arrays.stream(videoModes).filter(fullPredicate);
         return validModes.collect(Collectors.toList());
     }
@@ -102,16 +107,15 @@ public class USBCaptureProperties extends CaptureProperties {
         return videoModes;
     }
 
-    public VideoMode getVideoMode(int index){
+    public VideoMode getVideoMode(int index) {
         return videoModes.get(index);
     }
 
-    public VideoMode getCurrentVideoMode() { return staticProperties.mode; }
-
-    public int getCurrentVideoModeIndex(){
-        return getVideoModes().indexOf(getCurrentVideoMode());
+    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

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

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

@@ -12,5 +12,7 @@ public enum ImageRotationMode {
         this.value = value;
     }
 
-    public boolean isRotated(){return this.value==DEG_90.value||this.value==DEG_270.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

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

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

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

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

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

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

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

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

@@ -9,13 +9,16 @@ public class CaptureProperties {
     protected CaptureStaticProperties staticProperties;
     private Rotation2d tilt = new Rotation2d();
 
-    protected CaptureProperties() {
-    }
+    protected CaptureProperties() {}
 
     public CaptureProperties(VideoMode videoMode, double fov) {
         staticProperties = new CaptureStaticProperties(videoMode, fov);
     }
-    public void setStaticProperties(CaptureStaticProperties staticProperties) {this.staticProperties = staticProperties;}
+
+    public void setStaticProperties(CaptureStaticProperties staticProperties) {
+        this.staticProperties = staticProperties;
+    }
+
     public CaptureStaticProperties getStaticProperties() {
         return staticProperties;
     }

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

@@ -5,8 +5,9 @@ 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)
-     */
+    * 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

@@ -3,13 +3,12 @@ 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;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.imgcodecs.Imgcodecs;
 
 public class StaticImageCapture implements CameraCapture {
 

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

@@ -3,10 +3,7 @@ package com.chameleonvision._2.vision.pipeline;
 import com.chameleonvision._2.vision.camera.CameraCapture;
 import org.opencv.core.Mat;
 
-/**
- *
- * @param <R> Pipeline result type
- */
+/** @param <R> Pipeline result type */
 public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings> {
     protected Mat outputMat = new Mat();
     protected CameraCapture cameraCapture;
@@ -28,5 +25,6 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
         cameraCapture.setBrightness((int) settings.brightness);
         cameraCapture.setGain((int) settings.gain);
     }
-    abstract public R runPipeline(Mat inputMat);
+
+    public abstract R runPipeline(Mat inputMat);
 }

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

@@ -1,8 +1,7 @@
 package com.chameleonvision._2.vision.pipeline;
 
-import org.opencv.core.Mat;
-
 import java.util.List;
+import org.opencv.core.Mat;
 
 public abstract class CVPipelineResult<T> {
     public final List<T> targets;
@@ -14,7 +13,7 @@ public abstract class CVPipelineResult<T> {
     public CVPipelineResult(List<T> targets, Mat outputMat, long processTime) {
         this.targets = targets;
         hasTarget = targets != null && !targets.isEmpty();
-//        this.outputMat = outputMat;
+        //        this.outputMat = outputMat;
         outputMat.copyTo(this.outputMat);
         outputMat.release();
         this.processTime = processTime;

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

@@ -4,10 +4,8 @@ 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
-     */
+    * @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

@@ -13,7 +13,6 @@ 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;
@@ -28,14 +27,16 @@ public class PipelineManager {
     public final LinkedList<CVPipeline> pipelines = new LinkedList<>();
 
     public final CVPipeline driverModePipeline = new DriverVisionPipeline(new CVPipelineSettings());
-    public final Calibrate3dPipeline calib3dPipe = new Calibrate3dPipeline(new StandardCVPipelineSettings());
+    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) {
+    public PipelineManager(
+            VisionProcess visionProcess, List<CVPipelineSettings> loadedPipelineSettings) {
         parentProcess = visionProcess;
         if (loadedPipelineSettings == null || loadedPipelineSettings.size() == 0) {
             pipelines.add(new StandardCVPipeline("New Pipeline"));
@@ -115,7 +116,7 @@ public class PipelineManager {
         if (currentPipelineIndex == DRIVERMODE_INDEX) {
             return driverModePipeline;
         } else if (currentPipelineIndex <= CAL_3D_INDEX) {
-          return calib3dPipe;
+            return calib3dPipe;
         } else {
             return pipelines.get(currentPipelineIndex);
         }
@@ -135,25 +136,24 @@ public class PipelineManager {
 
             newPipeline = calib3dPipe;
         } else {
-            if (index < pipelines.size()&&index>=0) {
+            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");
             }
-            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)) {
+            if (ConfigManager.settings.currentCamera.equals(
+                    parentProcess.getCamera().getProperties().name)) {
                 ConfigManager.settings.currentPipeline = currentPipelineIndex;
 
                 HashMap<String, Object> pipeChange = new HashMap<>();
@@ -208,9 +208,10 @@ public class PipelineManager {
     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{
+        if (destinationProcess.pipelineManager.pipelines.stream()
+                .anyMatch(c -> c.settings.nickname.equals(pipeline.nickname))) {
+            //         throw new DuplicatedKeyException("key Already exists");
+        } else {
             destinationProcess.pipelineManager.addPipeline(pipeline);
         }
     }
@@ -237,15 +238,16 @@ public class PipelineManager {
         getConfig().saveDriverMode(driverModePipeline.settings);
     }
 
-    private static final Comparator<CVPipeline> IndexComparator = (o1, o2) -> {
-        int o1Index = o1.settings.index;
-        int o2Index = o2.settings.index;
+    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;
-    };
+                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

@@ -2,26 +2,27 @@ 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.chameleonvision._2.vision.pipeline.CVPipeline;
 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 java.util.ArrayList;
+import java.util.List;
 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> {
+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 MatOfPoint3f objP; // new MatOfPoint3f(checkerboardSquaresHigh + checkerboardSquaresWide,
+    // 3);//(checkerboardSquaresWide * checkerboardSquaresHigh, 3);
     private Size patternSize = new Size(checkerboardSquaresHigh, checkerboardSquaresWide);
     private Size imageSize;
     double checkerboardSquareSize = 1; // inches!
@@ -35,7 +36,9 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
     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 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;
@@ -47,8 +50,10 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
 
         objP = new MatOfPoint3f();
 
-        for(int i = 0; i < checkerboardSquaresHigh * checkerboardSquaresWide; i++) {
-            objP.push_back(new MatOfPoint3f(new Point3(i / checkerboardSquaresWide, i % checkerboardSquaresHigh, 0.0f)));
+        for (int i = 0; i < checkerboardSquaresHigh * checkerboardSquaresWide; i++) {
+            objP.push_back(
+                    new MatOfPoint3f(
+                            new Point3(i / checkerboardSquaresWide, i % checkerboardSquaresHigh, 0.0f)));
         }
 
         setSquareSize(checkerboardSquareSizeUnits);
@@ -78,15 +83,13 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
         Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_BGR2GRAY);
         var checkerboardFound = Calib3d.findChessboardCorners(inputMat, patternSize, calibrationOutput);
 
-
-
-        if(!checkerboardFound) {
+        if (!checkerboardFound) {
             Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_GRAY2BGR);
 
             return new DriverVisionPipeline.DriverPipelineResult(null, inputMat, 0);
         }
 
-//        System.out.println("[SolvePNP] checkerboard found!!");
+        //        System.out.println("[SolvePNP] checkerboard found!!");
 
         // cool we found a checkerboard
         // do corner subpixel
@@ -97,7 +100,7 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
         // draw the chessboard
         Calib3d.drawChessboardCorners(inputMat, patternSize, calibrationOutput, true);
 
-        if(wantsSnapshot) {
+        if (wantsSnapshot) {
             this.imageSize = new Size(inputMat.width(), inputMat.height());
 
             var mat = new MatOfPoint3f();
@@ -130,8 +133,10 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
         List<Mat> tvecs = new ArrayList<>();
 
         try {
-           calibrationAccuracy = Calib3d.calibrateCamera(objpoints, imgpoints, imageSize, cameraMatrix, distortionCoeffs, rvecs, tvecs);
-        } catch(Exception e) {
+            calibrationAccuracy =
+                    Calib3d.calibrateCamera(
+                            objpoints, imgpoints, imageSize, cameraMatrix, distortionCoeffs, rvecs, tvecs);
+        } catch (Exception e) {
             System.err.println("Camera calibration failed!");
             initPipeline(cameraCapture);
             return false;
@@ -139,32 +144,35 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
 
         VideoMode currentVidMode = cameraCapture.getCurrentVideoMode();
         Size resolution = new Size(currentVidMode.width, currentVidMode.height);
-        CameraCalibrationConfig cal = new CameraCalibrationConfig(resolution, cameraMatrix, distortionCoeffs, squareSizeInches);
+        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));
+            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){
+    public void setVideoMode(VideoMode mode) {
         this.calibrationMode = mode;
     }
-    
+
     public int getSnapshotCount() {
         return captureCount + 1;
     }
 
-    public double getCalibrationAccuracy(){
+    public double getCalibrationAccuracy() {
         return calibrationAccuracy;
     }
 }

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

@@ -8,17 +8,17 @@ 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 java.util.List;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.Mat;
 
-import java.util.List;
-
-
-public class DriverVisionPipeline extends CVPipeline<DriverVisionPipeline.DriverPipelineResult, CVPipelineSettings> {
+public class DriverVisionPipeline
+        extends CVPipeline<DriverVisionPipeline.DriverPipelineResult, CVPipelineSettings> {
 
     private RotateFlipPipe rotateFlipPipe;
     private Draw2dCrosshairPipe drawCrosshairPipe;
-    private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings crosshairPipeSettings = new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings();
+    private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings crosshairPipeSettings =
+            new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings();
 
     private final MemoryManager memoryManager = new MemoryManager(200, 20000);
 
@@ -31,8 +31,9 @@ public class DriverVisionPipeline extends CVPipeline<DriverVisionPipeline.Driver
     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);
+        crosshairPipeSettings.showCrosshair = true;
+        drawCrosshairPipe =
+                new Draw2dCrosshairPipe(crosshairPipeSettings, CalibrationMode.None, null, 0, 0);
     }
 
     @Override
@@ -41,7 +42,8 @@ public class DriverVisionPipeline extends CVPipeline<DriverVisionPipeline.Driver
         rotateFlipPipe.setConfig(settings.rotationMode, settings.flipMode);
 
         Pair<Mat, Long> rotateFlipResult = rotateFlipPipe.run(inputMat);
-        Pair<Mat, Long> draw2dCrosshairResult = drawCrosshairPipe.run(Pair.of(rotateFlipResult.getLeft(),null));
+        Pair<Mat, Long> draw2dCrosshairResult =
+                drawCrosshairPipe.run(Pair.of(rotateFlipResult.getLeft(), null));
         memoryManager.run();
 
         return new DriverPipelineResult(null, draw2dCrosshairResult.getLeft(), 0);

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

@@ -32,18 +32,17 @@ public class StandardCVPipelineSettings extends CVPipelineSettings {
     // 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)
-    );
+    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

@@ -23,18 +23,19 @@ public class BlurPipe implements Pipe<Mat, Mat> {
     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);
-//        }
+        //        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

@@ -7,15 +7,16 @@ 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 java.util.ArrayList;
+import java.util.List;
 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>> {
-
+public class Collect2dTargetsPipe
+        implements Pipe<
+                Pair<List<StandardCVPipeline.TrackedTarget>, CaptureStaticProperties>,
+                List<StandardCVPipeline.TrackedTarget>> {
 
     private CaptureStaticProperties camProps;
     private CalibrationMode calibrationMode;
@@ -26,11 +27,32 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.T
     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 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) {
+    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;
@@ -41,7 +63,8 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.T
     }
 
     @Override
-    public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(Pair<List<StandardCVPipeline.TrackedTarget>, CaptureStaticProperties> inputPair) {
+    public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(
+            Pair<List<StandardCVPipeline.TrackedTarget>, CaptureStaticProperties> inputPair) {
         long processStartNanos = System.nanoTime();
 
         targets.clear();
@@ -64,22 +87,26 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.T
 
                 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;
-                    }
+                    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;
 
@@ -114,7 +141,8 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.T
     }
 
     private double calculatePitch(double pixelY, double centerY) {
-        double pitch = FastMath.toDegrees(FastMath.atan((pixelY - centerY) / camProps.verticalFocalLength));
+        double pitch =
+                FastMath.toDegrees(FastMath.atan((pixelY - centerY) / camProps.verticalFocalLength));
         return (pitch * -1);
     }
 

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

@@ -4,16 +4,16 @@ 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;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.*;
+import org.opencv.core.Point;
+import org.opencv.imgproc.Imgproc;
 
-public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
+public class Draw2dContoursPipe
+        implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
 
     private final Draw2dContoursSettings settings;
     private CaptureStaticProperties camProps;
@@ -27,13 +27,12 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<StandardCVPipelin
     @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) {
+    public void setConfig(boolean showMultiple, CaptureStaticProperties captureProps) {
         settings.showMultiple = showMultiple;
         camProps = captureProps;
     }
@@ -43,12 +42,12 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<StandardCVPipelin
         long processStartNanos = System.nanoTime();
 
         if (settings.showCentroid || settings.showMaximumBox || settings.showRotatedBox) {
-//            input.getLeft().copyTo(processBuffer);
-//            processBuffer = input.getLeft();
+            //            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){
+                    if (i != 0 && !settings.showMultiple) {
                         break;
                     }
                     StandardCVPipeline.TrackedTarget target = input.getRight().get(i);
@@ -61,31 +60,44 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<StandardCVPipelin
 
                     r.points(vertices);
                     contour.fromArray(vertices);
-//                    MatOfPoint contour = new MatOfPoint(vertices);
+                    //                    MatOfPoint contour = new MatOfPoint(vertices);
                     drawnContours.add(contour);
 
-
                     if (settings.showRotatedBox) {
-                        Imgproc.drawContours(input.getLeft(), drawnContours, 0, ColorHelper.colorToScalar(settings.rotatedBoxColor), settings.boxOutlineSize);
+                        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);
+                        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);
-
+                        Imgproc.circle(
+                                input.getLeft(),
+                                target.point,
+                                3,
+                                ColorHelper.colorToScalar(settings.centroidColor),
+                                2);
                     }
 
-//                    contour.release();
+                    //                    contour.release();
                 }
             }
 
-//            processBuffer.copyTo(outputMat);
-//            processBuffer.release();
+            //            processBuffer.copyTo(outputMat);
+            //            processBuffer.release();
         } else {
-//            input.getLeft().copyTo(outputMat);
+            //            input.getLeft().copyTo(outputMat);
         }
         long processTime = System.nanoTime() - processStartNanos;
         return Pair.of(input.getLeft(), processTime);

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

@@ -5,30 +5,38 @@ 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 java.awt.*;
+import java.util.List;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.Mat;
 import org.opencv.core.Point;
 import org.opencv.imgproc.Imgproc;
 
-import java.awt.*;
-import java.util.List;
+public class Draw2dCrosshairPipe
+        implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
 
-public class Draw2dCrosshairPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
-
-    //Settings
+    // 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) {
+    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) {
+    public void setConfig(
+            Draw2dCrosshairPipeSettings crosshairSettings,
+            CalibrationMode calibrationMode,
+            DoubleCouple calibrationPoint,
+            double calibrationM,
+            double calibrationB) {
         this.crosshairSettings = crosshairSettings;
         this.calibrationMode = calibrationMode;
         this.calibrationPoint = calibrationPoint;
@@ -40,7 +48,7 @@ public class Draw2dCrosshairPipe implements Pipe<Pair<Mat, List<StandardCVPipeli
     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();
+        //        List<StandardCVPipeline.TrackedTarget> targets = inputPair.getRight();
         double x, y;
         double scale = image.cols() / 32.0;
 
@@ -50,28 +58,37 @@ public class Draw2dCrosshairPipe implements Pipe<Pair<Mat, List<StandardCVPipeli
             y = image.rows() / 2.0;
             switch (this.calibrationMode) {
                 case Single:
-                    if(this.calibrationPoint.equals(new DoubleCouple()))
-                    {
+                    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;
+                    //                    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);
+            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;

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

@@ -5,18 +5,18 @@ 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 java.awt.*;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.stream.Collectors;
 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> {
+public class DrawSolvePNPPipe
+        implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
 
     private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
 
@@ -25,22 +25,22 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
     public Scalar red = ColorHelper.colorToScalar(Color.RED);
     public Scalar orange = ColorHelper.colorToScalar(Color.orange);
 
-    public DrawSolvePNPPipe(StandardCVPipelineSettings standardCVPipelineSettings, CameraCalibrationConfig settings) {
+    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 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);
     }
@@ -71,7 +71,15 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
         for (var it : targets.getRight()) {
 
             try {
-                Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat(), 0);
+                Calib3d.projectPoints(
+                        boxCornerMat,
+                        it.rVector,
+                        it.tVector,
+                        this.cameraMatrix,
+                        this.distortionCoefficients,
+                        imagePoints,
+                        new Mat(),
+                        0);
             } catch (Exception e) {
                 e.printStackTrace();
             }

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

@@ -29,7 +29,7 @@ public class ErodeDilatePipe implements Pipe<Mat, Mat> {
         long processStartNanos = System.nanoTime();
 
         if (erode || dilate) {
-//            input.copyTo(processBuffer);
+            //            input.copyTo(processBuffer);
 
             if (erode) {
                 Imgproc.erode(input, input, kernel);
@@ -39,10 +39,10 @@ public class ErodeDilatePipe implements Pipe<Mat, Mat> {
                 Imgproc.dilate(input, input, kernel);
             }
 
-//            processBuffer.copyTo(outputMat);
-//            processBuffer.release();
+            //            processBuffer.copyTo(outputMat);
+            //            processBuffer.release();
         } else {
-//            input.copyTo(outputMat);
+            //            input.copyTo(outputMat);
         }
 
         long processTime = System.nanoTime() - processStartNanos;

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

@@ -5,15 +5,12 @@ import com.chameleonvision._2.vision.pipeline.Pipe;
 import com.chameleonvision.common.util.math.MathUtils;
 import com.chameleonvision.common.util.numbers.DoubleCouple;
 import com.chameleonvision.common.vision.opencv.Contour;
-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.core.RotatedRect;
-import org.opencv.imgproc.Imgproc;
-
 import java.util.ArrayList;
 import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.Rect;
+import org.opencv.core.RotatedRect;
 
 public class FilterContoursPipe implements Pipe<List<MatOfPoint>, List<Contour>> {
 
@@ -24,14 +21,22 @@ public class FilterContoursPipe implements Pipe<List<MatOfPoint>, List<Contour>>
 
     private List<Contour> filteredContours = new ArrayList<>();
 
-    public FilterContoursPipe(DoubleCouple area, DoubleCouple ratio, DoubleCouple extent, CaptureStaticProperties camProps) {
+    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) {
+    public void setConfig(
+            DoubleCouple area,
+            DoubleCouple ratio,
+            DoubleCouple extent,
+            CaptureStaticProperties camProps) {
         this.area = area;
         this.ratio = ratio;
         this.extent = extent;
@@ -61,7 +66,7 @@ public class FilterContoursPipe implements Pipe<List<MatOfPoint>, List<Contour>>
 
         // AspectRatio filtering
         Rect boundingRect = realContour.getBoundingRect();
-        double aspectRatio = ((double)boundingRect.width / boundingRect.height);
+        double aspectRatio = ((double) boundingRect.width / boundingRect.height);
         if (aspectRatio < ratio.getFirst() || aspectRatio > ratio.getSecond()) {
             return;
         }

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

@@ -2,15 +2,14 @@ package com.chameleonvision._2.vision.pipeline.pipes;
 
 import com.chameleonvision._2.vision.pipeline.Pipe;
 import com.chameleonvision.common.vision.opencv.Contour;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.stream.Collectors;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfPoint;
 import org.opencv.imgproc.Imgproc;
 
-import java.util.ArrayList;
-import java.util.List;
-import java.util.stream.Collectors;
-
 public class FindContoursPipe implements Pipe<Mat, List<Contour>> {
 
     private List<MatOfPoint> foundContours = new ArrayList<>();
@@ -23,9 +22,11 @@ public class FindContoursPipe implements Pipe<Mat, List<Contour>> {
 
         foundContours.clear();
 
-        Imgproc.findContours(input, foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1);
+        Imgproc.findContours(
+                input, foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1);
 
         long processTime = System.nanoTime() - processStartNanos;
-        return Pair.of(foundContours.stream().map(Contour::new).collect(Collectors.toList()), processTime);
+        return Pair.of(
+                foundContours.stream().map(Contour::new).collect(Collectors.toList()), processTime);
     }
 }

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

@@ -5,17 +5,17 @@ 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 java.util.ArrayList;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
 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>> {
+public class GroupContoursPipe
+        implements Pipe<List<MatOfPoint>, List<StandardCVPipeline.TrackedTarget>> {
 
     private static final Comparator<MatOfPoint> sortByMomentsX =
             Comparator.comparingDouble(GroupContoursPipe::calcMomentsX);
@@ -55,76 +55,78 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
             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();
-                        }
+                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;
                     }
-                    break;
-                }
             }
         }
 
@@ -160,36 +162,40 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
             double massX = (x0A + x0B) / 2;
             double massY = (y0A + y0B) / 2;
             switch (intersection) {
-                case Up: {
-                    if (intersectionY < massY) {
+                case Up:
+                    {
+                        if (intersectionY < massY) {
                             return true;
+                        }
+                        break;
                     }
-                    break;
-                }
-                case Down: {
-                    if (intersectionY > massY) {
+                case Down:
+                    {
+                        if (intersectionY > massY) {
                             return true;
-                    }
+                        }
 
-                    break;
-                }
-                case Left: {
-                    if (intersectionX < massX) {
+                        break;
+                    }
+                case Left:
+                    {
+                        if (intersectionX < massX) {
 
-                        return true;
+                            return true;
+                        }
+                        break;
                     }
-                    break;
-                }
-                case Right: {
-                    if (intersectionX > massX) {
-                        return true;
+                case Right:
+                    {
+                        if (intersectionX > massX) {
+                            return true;
+                        }
+                        break;
                     }
-                    break;
-                }
             }
             return false;
         } catch (Exception e) {
             return false;
         }
     }
-}
+}

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

@@ -43,4 +43,3 @@ public class HsvPipe implements Pipe<Mat, Mat> {
         return Pair.of(outputMat, processTime);
     }
 }
-

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

@@ -22,11 +22,10 @@ public class OutputMatPipe implements Pipe<Pair<Mat, Mat>, Mat> {
     }
 
     /**
-     *
-     * @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
-     */
+    * @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();

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

@@ -1,8 +1,8 @@
 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 com.chameleonvision._2.vision.pipeline.Pipe;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.Core;
 import org.opencv.core.Mat;
@@ -33,7 +33,7 @@ public class RotateFlipPipe implements Pipe<Mat, Mat> {
         boolean shouldRotate = !rotation.equals(ImageRotationMode.DEG_0);
 
         if (shouldFlip || shouldRotate) {
-//            input.copyTo(processBuffer);
+            //            input.copyTo(processBuffer);
 
             if (shouldFlip) {
                 Core.flip(input, input, flip.value);
@@ -43,10 +43,10 @@ public class RotateFlipPipe implements Pipe<Mat, Mat> {
                 Core.rotate(input, input, rotation.value);
             }
 
-//            processBuffer.copyTo(outputMat);
-//            processBuffer.release();
+            //            processBuffer.copyTo(outputMat);
+            //            processBuffer.release();
         } else {
-//            input.copyTo(outputMat);
+            //            input.copyTo(outputMat);
         }
 
         long processTime = System.nanoTime() - processStartNanos;

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

@@ -1,12 +1,5 @@
 package com.chameleonvision._2.vision.pipeline.pipes;
 
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Comparator;
-import java.util.List;
-import java.util.NoSuchElementException;
-import java.util.stream.Collectors;
-
 import com.chameleonvision._2.config.CameraCalibrationConfig;
 import com.chameleonvision._2.vision.pipeline.Pipe;
 import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline;
@@ -14,6 +7,12 @@ 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 java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Comparator;
+import java.util.List;
+import java.util.NoSuchElementException;
+import java.util.stream.Collectors;
 import org.apache.commons.lang3.tuple.Pair;
 import org.apache.commons.math3.util.FastMath;
 import org.opencv.calib3d.Calib3d;
@@ -30,477 +29,506 @@ import org.opencv.core.Size;
 import org.opencv.core.TermCriteria;
 import org.opencv.imgproc.Imgproc;
 
-/**
- * Handles detecting target corners and calculating robot-relative pose.
- */
-public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, Mat>,
-        List<StandardCVPipeline.TrackedTarget>> {
+/** Handles detecting target corners and calculating robot-relative pose. */
+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> targetList = 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;
+    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> targetList = 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;
 
-  /**
-   * @param settings    unused :bolb:
-   * @param calibration the camera intrinsics and extrinsics
-   * @param tilt        The pitch of the camera relative to horzontal. used to account for
-   *                    distances in calculate pose
-   */
-  public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration,
-                      Rotation2d tilt) {
-    super();
-    setCameraCoeffs(calibration);
-//        setBoundingBoxTarget(settings.targetWidth, settings.targetHeight);
-    // TODO add proper year differentiation
-    set2020Target(true);
+    /**
+    * @param settings unused :bolb:
+    * @param calibration the camera intrinsics and extrinsics
+    * @param tilt The pitch of the camera relative to horzontal. used to account for distances in
+    *     calculate pose
+    */
+    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);
-    targetList.clear();
-    for (var target : targets) {
-      MatOfPoint2f corners;
-      // if it's a dual target use 2019, but default to 2020
-      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;
-
-      // convert the corners into a Pose2d
-      var pose = calculatePose(corners, target);
-      targetList.add(pose); // TODO null check null poses. DO NOT ADD A NULL CHECK HERE, otherwise
-      // the order will be wrong.
-    }
-    long processTime = System.nanoTime() - processStartNanos;
-    return Pair.of(targetList, processTime);
-  }
-
-  /**
-   * basically we split the target's two tapes, find the min area rectangle for each, and take
-   * the outermost 4 corners out of the 2 rectangles
-   *
-   * @param target the target to use
-   * @return the 4 outermost corners.
-   */
-  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;
+        this.tilt_angle = tilt.getRadians();
     }
 
-    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. Uses approxPolyDP to
-   * approximate the target outline.
-   *
-   * @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();
-
-      // bottom left and bottom right have to be in the correct quadrant and are the furthest
-      // from the center
-      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;
+    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);
+        }
     }
 
-    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));
+    public void setBoundingBoxTarget(double targetWidth, double targetHeight) {
+        // order is left top, left bottom, right bottom, right top
 
-    // 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());
-      }
+        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);
     }
 
-    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.
-   * <p>
-   * 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");
-
-  /**
-   * Calculate the pose of the vision target
-   *
-   * @param imageCornerPoints the corners we found.
-   * @param target            the target to process, mutated.
-   * @return the target, with the pose2d added to it.
-   */
-  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;
+    public void setObjectCorners(List<Point3> objectCorners) {
+        objPointsMat.release();
+        objPointsMat = new MatOfPoint3f();
+        objPointsMat.fromList(objectCorners);
     }
 
-    imageCornerPoints.copyTo(target.imageCornerPoints);
-
-    try {
-      Calib3d.solvePnP(objPointsMat, imageCornerPoints, cameraMatrix, distortionCoefficients,
-              rVec, tVec);
-    } catch (Exception e) {
-      e.printStackTrace();
-      return null;
+    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();
     }
 
-//        tvecE.setString(tVec.dump());
-//        rvecE.setString(rVec.dump());
+    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;
+    }
 
-    // Algorithm from team 5190 Green Hope Falcons. Can also be found in Ligerbot's vision
-    // whitepaper
+    @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);
+        targetList.clear();
+        for (var target : targets) {
+            MatOfPoint2f corners;
+            // if it's a dual target use 2019, but default to 2020
+            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;
 
-    // the left/right distance to the target, unchanged by tilt. Inches
-    var x = tVec.get(0, 0)[0];
+            // convert the corners into a Pose2d
+            var pose = calculatePose(corners, target);
+            targetList.add(pose); // TODO null check null poses. DO NOT ADD A NULL CHECK HERE, otherwise
+            // the order will be wrong.
+        }
+        long processTime = System.nanoTime() - processStartNanos;
+        return Pair.of(targetList, processTime);
+    }
 
-    // Z distance in the flat plane is given by
-    // Z_field = z cos theta + y sin theta.
-    // Z is the distance "out" of the camera (straight forward). Inches.
-    var z = tVec.get(2, 0)[0] * FastMath.cos(tilt_angle) + tVec.get(1, 0)[0] * FastMath.sin(tilt_angle);
+    /**
+    * basically we split the target's two tapes, find the min area rectangle for each, and take the
+    * outermost 4 corners out of the 2 rectangles
+    *
+    * @param target the target to use
+    * @return the 4 outermost corners.
+    */
+    private MatOfPoint2f findCorner2019(StandardCVPipeline.TrackedTarget target) {
+        if (target.leftRightDualTargetPair == null) return null;
 
-    Calib3d.Rodrigues(rVec, rodriguez);
-    Core.transpose(rodriguez, rot_inv); // rodrigurz.t()
+        var left = target.leftRightDualTargetPair.getLeft();
+        var right = target.leftRightDualTargetPair.getRight();
 
-    scaledTvec = matScale(tVec, -1);
-    Core.gemm(rot_inv, scaledTvec, 1, kMat, 0, pzero_world);
+        // flip if the "left" target is to the right
+        if (left.x > right.x) {
+            var temp = left;
+            left = right;
+            right = temp;
+        }
 
-    var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.get(2, 0)[0]);
+        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;
+    }
 
-    // target rotation is the rotation of the target relative to straight ahead. this number
-    // should be unchanged if the robot purely translated left/right.
-    var targetRotation = -angle2; // radians
+    MatOfPoint2f target2020ResultMat = new MatOfPoint2f();
 
-    // 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
+    private double distanceBetween(Point a, Point b) {
+        return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2));
+    }
 
-    //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;
+    /**
+    * Find the target using the outermost tape corners and a 2020 target. Uses approxPolyDP to
+    * approximate the target outline.
+    *
+    * @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;
 
-    return target;
-  }
+        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)));
 
-  /**
-   * 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;
-  }
+        // 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();
+
+            // bottom left and bottom right have to be in the correct quadrant and are the furthest
+            // from the center
+            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) {
+        // 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.
+    *
+    * <p>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");
+
+    /**
+    * Calculate the pose of the vision target
+    *
+    * @param imageCornerPoints the corners we found.
+    * @param target the target to process, mutated.
+    * @return the target, with the pose2d added to it.
+    */
+    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. Can also be found in Ligerbot's vision
+        // whitepaper
+
+        // the left/right distance to the target, unchanged by tilt. Inches
+        var x = tVec.get(0, 0)[0];
+
+        // Z distance in the flat plane is given by
+        // Z_field = z cos theta + y sin theta.
+        // Z is the distance "out" of the camera (straight forward). Inches.
+        var z =
+                tVec.get(2, 0)[0] * FastMath.cos(tilt_angle) + tVec.get(1, 0)[0] * FastMath.sin(tilt_angle);
+
+        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]);
+
+        // target rotation is the rotation of the target relative to straight ahead. this number
+        // should be unchanged if the robot purely translated left/right.
+        var targetRotation = -angle2; // radians
+
+        // We want a vector that is X forward and Y left.
+        // We have a Z_field (out of the camera projected onto the field), and an X left/right.
+        // so Z_field becomes X, and X becomes Y
+
+        //noinspection SuspiciousNameCombination
+        var targetLocation = new Translation2d(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

@@ -4,25 +4,34 @@ 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;
+import org.apache.commons.lang3.tuple.Pair;
+import org.apache.commons.math3.util.FastMath;
 
-public class SortContoursPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>, List<StandardCVPipeline.TrackedTarget>> {
+public class SortContoursPipe
+        implements Pipe<
+                List<StandardCVPipeline.TrackedTarget>, List<StandardCVPipeline.TrackedTarget>> {
 
-    private final Comparator<StandardCVPipeline.TrackedTarget> SortByCentermostComparator = Comparator.comparingDouble(this::calcSquareCenterDistance);
+    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> 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();
+    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();
+    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;
@@ -43,7 +52,8 @@ public class SortContoursPipe implements Pipe<List<StandardCVPipeline.TrackedTar
     }
 
     @Override
-    public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<StandardCVPipeline.TrackedTarget> input) {
+    public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(
+            List<StandardCVPipeline.TrackedTarget> input) {
         long processStartNanos = System.nanoTime();
 
         sortedContours.clear();
@@ -78,8 +88,11 @@ public class SortContoursPipe implements Pipe<List<StandardCVPipeline.TrackedTar
             }
         }
 
-        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);
+        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<>();
 
@@ -88,6 +101,8 @@ public class SortContoursPipe implements Pipe<List<StandardCVPipeline.TrackedTar
     }
 
     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));
+        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

@@ -1,13 +1,12 @@
 package com.chameleonvision._2.vision.pipeline.pipes;
 
 import com.chameleonvision._2.vision.pipeline.Pipe;
+import java.util.ArrayList;
+import java.util.List;
 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;

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

@@ -19,8 +19,6 @@ 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;
@@ -30,6 +28,7 @@ import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
+import org.opencv.core.Point3;
 
 public class RequestHandler {
 
@@ -56,7 +55,13 @@ public class RequestHandler {
             // 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);
+            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 {
@@ -77,9 +82,15 @@ public class RequestHandler {
             int cameraIndex = (Integer) data.getOrDefault("camera", -1);
 
             var pipelineIndex = (Integer) data.get("pipeline");
-            StandardCVPipelineSettings origPipeline = (StandardCVPipelineSettings) VisionManager.getCurrentUIVisionProcess().pipelineManager.getPipeline(pipelineIndex).settings;
+            StandardCVPipelineSettings origPipeline =
+                    (StandardCVPipelineSettings)
+                            VisionManager.getCurrentUIVisionProcess()
+                                    .pipelineManager
+                                    .getPipeline(pipelineIndex)
+                                    .settings;
             String tmp = objectMapper.writeValueAsString(origPipeline);
-            StandardCVPipelineSettings newPipeline = objectMapper.readValue(tmp, StandardCVPipelineSettings.class);
+            StandardCVPipelineSettings newPipeline =
+                    objectMapper.readValue(tmp, StandardCVPipelineSettings.class);
 
             if (cameraIndex == -1) { // same camera
 
@@ -92,12 +103,17 @@ public class RequestHandler {
                         newPipeline.videoModeIndex = cam.getCamera().getProperties().videoModes.size() - 1;
                     }
                     if (newPipeline.is3D) {
-                        var calibration = cam.getCamera().getCalibration(cam.getCamera().getProperties().getVideoMode(newPipeline.videoModeIndex));
+                        var calibration =
+                                cam.getCamera()
+                                        .getCalibration(
+                                                cam.getCamera().getProperties().getVideoMode(newPipeline.videoModeIndex));
                         if (calibration == null) {
                             newPipeline.is3D = false;
                         }
                     }
-                    VisionManager.getCurrentUIVisionProcess().pipelineManager.duplicatePipeline(newPipeline, cam);
+                    VisionManager.getCurrentUIVisionProcess()
+                            .pipelineManager
+                            .duplicatePipeline(newPipeline, cam);
                     ctx.status(200);
                 } else {
                     ctx.status(500);
@@ -108,7 +124,6 @@ public class RequestHandler {
         }
     }
 
-
     public static void onCameraSettings(Context ctx) {
         ObjectMapper objectMapper = kObjectMapper;
         try {
@@ -152,13 +167,15 @@ public class RequestHandler {
         }
         // convert from mm to meters
         pipeManager.calib3dPipe.setSquareSize(squareSize);
-        VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe.settings.videoModeIndex = resolutionIndex;
+        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;
+        Calibrate3dPipeline calPipe =
+                VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe;
 
         calPipe.takeSnapshot();
 
@@ -213,8 +230,14 @@ public class RequestHandler {
                 pointsList.add(pointToAdd);
             }
             System.out.println(pointsList.toString());
-            if (VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings instanceof StandardCVPipelineSettings) {
-                var settings = (StandardCVPipelineSettings) VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings;
+            if (VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings
+                    instanceof StandardCVPipelineSettings) {
+                var settings =
+                        (StandardCVPipelineSettings)
+                                VisionManager.getCurrentUIVisionProcess()
+                                        .pipelineManager
+                                        .getCurrentPipeline()
+                                        .settings;
                 settings.targetCornerMat.fromList(pointsList);
             }
         } catch (Exception e) {
@@ -235,7 +258,10 @@ public class RequestHandler {
                     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
+                    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);

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

@@ -9,25 +9,32 @@ public class Server {
     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);
-            });
-        });
+        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);

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

@@ -20,9 +20,6 @@ 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;
@@ -30,7 +27,8 @@ import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
 import java.util.stream.Collectors;
-
+import org.apache.commons.lang3.ArrayUtils;
+import org.msgpack.jackson.dataformat.MessagePackFactory;
 
 public class SocketHandler {
 
@@ -55,144 +53,179 @@ public class SocketHandler {
 
     @SuppressWarnings("unchecked")
     void onBinaryMessage(WsBinaryMessageContext context) throws Exception {
-        Map<String, Object> deserialized = objectMapper.readValue((byte[]) ArrayUtils.toPrimitive(context.data()),
-                new TypeReference<>() {
-                });
+        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());
+                //                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"));
+                    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;
+                            VisionManager.saveCurrentCameraDriverMode();
+                            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;
-                            }
-
+                    case "changeCameraName":
+                        {
+                            currentProcess.setCameraNickname((String) entry.getValue());
+                            sendFullSettings();
+                            VisionManager.saveCurrentCameraSettings();
+                            break;
                         }
-
-
-                        if (currentProcess.pipelineManager.getDriverMode()) {
-                            setField(currentProcess.pipelineManager.driverModePipeline.settings, entry.getKey(), entry.getValue());
-                        } else {
-                            setField(currentPipeline.settings, entry.getKey(), entry.getValue());
+                    case "changePipelineName":
+                        {
+                            currentProcess.pipelineManager.renameCurrentPipeline((String) entry.getValue());
+                            sendFullSettings();
+                            VisionManager.saveCurrentCameraPipelines();
+                            break;
                         }
-
-                        //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;
-                            }
+                    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;
+                                    }
+                            }
 
-                        VisionManager.saveCurrentCameraPipelines();
-                        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());
@@ -206,8 +239,7 @@ public class SocketHandler {
             Field field = obj.getClass().getField(fieldName);
             if (field.getType().isEnum())
                 field.set(obj, field.getType().getEnumConstants()[(Integer) value]);
-            else
-                field.set(obj, value);
+            else field.set(obj, value);
         } catch (NoSuchFieldException | IllegalAccessException ex) {
             ex.printStackTrace();
         }
@@ -233,17 +265,32 @@ public class SocketHandler {
     }
 
     public static void broadcastMessage(Object obj) {
-        broadcastMessage(obj, null);//Broadcasts the message to every user
+        broadcastMessage(obj, null); // Broadcasts the message to every user
     }
 
-    private static HashMap<String, Object> getOrdinalPipeline(Class cvClass) throws IllegalAccessException {
+    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));
+                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);
+                    var ordinal =
+                            (Enum)
+                                    field.get(
+                                            VisionManager.getCurrentUIVisionProcess()
+                                                    .pipelineManager
+                                                    .getCurrentPipeline()
+                                                    .settings);
                     tmp.put(field.getName(), ordinal.ordinal());
                 }
             } catch (IllegalArgumentException e) {
@@ -271,18 +318,21 @@ public class SocketHandler {
 
         tmp.put("fov", currentCamera.getProperties().getFOV());
         tmp.put("streamDivisor", currentVisionProcess.cameraStreamer.getDivisor().ordinal());
-        tmp.put("resolution", currentVisionProcess.getCamera().getProperties().getCurrentVideoModeIndex());
+        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());
+        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
+        // General settings
         Map<String, Object> fullSettings = new HashMap<>();
 
         VisionProcess currentProcess = VisionManager.getCurrentUIVisionProcess();
@@ -296,7 +346,9 @@ public class SocketHandler {
             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(
+                    "currentPipelineIndex",
+                    VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipelineIndex());
             fullSettings.put("currentCameraIndex", VisionManager.getCurrentUIVisionProcessIndex());
         } catch (IllegalAccessException e) {
             System.err.println("No camera found!");

chameleon-server/src/main/java/com/chameleonvision/common/calibration/CameraCalibrationCoefficients.java

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

chameleon-server/src/main/java/com/chameleonvision/common/calibration/JsonMat.java

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

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

@@ -6,11 +6,15 @@ import edu.wpi.first.networktables.LogMessage;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import java.util.function.Consumer;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 public class NetworkTablesManager {
 
     private NetworkTablesManager() {}
 
+    private static final Logger logger = LoggerFactory.getLogger(NetworkTablesManager.class);
+
     private static final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
 
     public static final String kRootTableName = "/chameleon-vision";
@@ -32,10 +36,10 @@ public class NetworkTablesManager {
         @Override
         public void accept(LogMessage logMessage) {
             if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) {
-                System.err.println("NT Connection has failed! Will retry in background.");
+                logger.error("NT Connection has failed! Will retry in background.");
                 hasReportedConnectionFailure = true;
             } else if (logMessage.message.contains("connected")) {
-                System.out.println("NT Connected!");
+                logger.info("NT Connected!");
                 hasReportedConnectionFailure = false;
                 ScriptManager.queueEvent(ScriptEventType.kNTConnected);
             }
@@ -48,16 +52,16 @@ public class NetworkTablesManager {
 
     public static void setClientMode(String host) {
         isServer = false;
-        System.out.println("Starting NT Client");
+        logger.info("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!");
+                logger.info("[NetworkTablesManager] Connected to the robot!");
             } else {
-                System.out.println(
+                logger.info(
                         "[NetworkTablesManager] Could NOT to the robot! Will retry in the background...");
             }
         }
@@ -69,7 +73,7 @@ public class NetworkTablesManager {
 
     public static void setServerMode() {
         isServer = true;
-        System.out.println("Starting NT Server");
+        logger.info("Starting NT Server");
         ntInstance.stopClient();
         ntInstance.startServer();
     }

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

@@ -1,20 +0,0 @@
-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

@@ -9,13 +9,18 @@ import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
 import org.apache.commons.io.FileUtils;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 public class LinuxNetworking extends SysNetworking {
     private static final String PATH = "/etc/dhcpcd.conf";
 
+    private Logger logger = LoggerFactory.getLogger(LinuxNetworking.class);
+
     @Override
     public boolean setDHCP() {
         File dhcpConf = new File(PATH);
+        logger.debug("Removing static IP from {}", PATH);
         if (dhcpConf.exists()) {
             try {
                 List<String> lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8);
@@ -44,7 +49,7 @@ public class LinuxNetworking extends SysNetworking {
             }
 
         } else {
-            System.err.println("dhcpcd5 is not installed, unable to set IP.");
+            logger.error("dhcpcd5 is not installed, unable to set IP.");
             return false;
         }
         return true;

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

@@ -1,14 +1,15 @@
 package com.chameleonvision.common.scripting;
 
-import com.chameleonvision.common.logging.DebugLogger;
 import com.chameleonvision.common.util.ShellExec;
 import java.io.IOException;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 public class ScriptEvent {
-    private static final DebugLogger logger = new DebugLogger(true);
     private static final ShellExec executor = new ShellExec(true, true);
 
     public final ScriptConfig config;
+    private final Logger logger = LoggerFactory.getLogger(ScriptEvent.class);
 
     public ScriptEvent(ScriptConfig config) {
         this.config = config;
@@ -23,12 +24,13 @@ public class ScriptEvent {
         if (!error.isEmpty()) {
             System.err.printf("Error when running \"%s\" script: %s\n", config.eventType.name(), error);
         } else if (!output.isEmpty()) {
-            logger.printInfo(
+            logger.info(
                     String.format("Output from \"%s\" script: %s\n", config.eventType.name(), output));
         }
-        logger.printInfo(
+        logger.info(
                 String.format(
-                        "Script for %s ran with command line: \"%s\", exit code: %d, output: %s, error: %s\n",
+                        "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/ScriptManager.java

@@ -1,6 +1,5 @@
 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;
@@ -11,10 +10,12 @@ import java.nio.file.Paths;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.concurrent.LinkedBlockingDeque;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 public class ScriptManager {
 
-    private static DebugLogger logger = new DebugLogger(true);
+    private static final Logger logger = LoggerFactory.getLogger(ScriptManager.class);
 
     private ScriptManager() {}
 
@@ -124,7 +125,7 @@ public class ScriptManager {
         if (!Platform.CurrentPlatform.isWindows()) {
             try {
                 queuedEvents.putLast(eventType);
-                logger.printInfo("Queued event: " + eventType.name());
+                logger.info("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/util/ReflectionUtils.java

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

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

@@ -0,0 +1,123 @@
+package com.chameleonvision.common.util;
+
+import edu.wpi.cscore.CameraServerCvJNI;
+import java.io.File;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.util.Optional;
+import org.opencv.core.Mat;
+import org.opencv.highgui.HighGui;
+
+public class TestUtils {
+
+    public enum WPI2019Image {
+        kCargoAngledDark48in(1.2192),
+        kCargoSideStraightDark36in(0.9144),
+        kCargoSideStraightDark60in(1.524),
+        kCargoSideStraightDark72in(1.8288),
+        kCargoSideStraightPanelDark36in(0.9144),
+        kCargoStraightDark19in(0.4826),
+        kCargoStraightDark24in(0.6096),
+        kCargoStraightDark48in(1.2192),
+        kCargoStraightDark72in(1.8288),
+        kCargoStraightDark72in_HighRes(1.8288),
+        kCargoStraightDark90in(2.286);
+
+        public static double FOV = 68.5;
+
+        public final double distanceMeters;
+        public final String path;
+
+        String getPath() {
+            var filename = this.toString().substring(1);
+            return "\\2019\\WPI\\" + filename + ".jpg";
+        }
+
+        WPI2019Image(double distanceMeters) {
+            this.distanceMeters = distanceMeters;
+            this.path = getPath();
+        }
+    }
+
+    public enum WPI2020Image {
+        kBlueGoal_060in_Center(1.524),
+        kBlueGoal_084in_Center(2.1336),
+        kBlueGoal_108in_Center(2.7432),
+        kBlueGoal_132in_Center(3.3528),
+        kBlueGoal_156in_Center(3.9624),
+        kBlueGoal_180in_Center(4.572),
+        kBlueGoal_156in_Left(3.9624),
+        kBlueGoal_224in_Left(5.6896),
+        kBlueGoal_228in_ProtectedZone(5.7912),
+        kBlueGoal_330in_ProtectedZone(8.382),
+        kBlueGoal_Far_ProtectedZone(10.668), // TODO: find a more accurate distance
+        kRedLoading_016in_Down(0.4064),
+        kRedLoading_030in_Down(0.762),
+        kRedLoading_048in_Down(1.2192),
+        kRedLoading_048in(1.2192),
+        kRedLoading_060in(1.524),
+        kRedLoading_084in(2.1336),
+        kRedLoading_108in(2.7432);
+
+        public static double FOV = 68.5;
+
+        public final double distanceMeters;
+        public final String path;
+
+        String getPath() {
+            var filename = this.toString().substring(1).replace('_', '-');
+            return "\\2020\\WPI\\" + filename + ".jpg";
+        }
+
+        WPI2020Image(double distanceMeters) {
+            this.distanceMeters = distanceMeters;
+            this.path = getPath();
+        }
+    }
+
+    private static Path getTestImagesPath() {
+        var folder = TestUtils.class.getClassLoader().getResource("testimages");
+        return Optional.ofNullable(folder).map(url -> new File(url.getFile()).toPath()).orElse(null);
+    }
+
+    public static Path getCalibrationPath() {
+        var folder = TestUtils.class.getClassLoader().getResource("calibration");
+        return Optional.ofNullable(folder).map(url -> new File(url.getFile()).toPath()).orElse(null);
+    }
+
+    public static Path getWPIImagePath(WPI2020Image image) {
+        return Path.of(getTestImagesPath().toString(), image.path);
+    }
+
+    public static Path getWPIImagePath(WPI2019Image image) {
+        return Path.of(getTestImagesPath().toString(), image.path);
+    }
+
+    public static void loadLibraries() {
+        try {
+            CameraServerCvJNI.forceLoad();
+        } catch (IOException e) {
+            // ignored
+        }
+    }
+
+    private static int DefaultTimeoutMillis = 5000;
+
+    public static void showImage(Mat frame, String title, int timeoutMs) {
+        HighGui.imshow(title, frame);
+        HighGui.waitKey(timeoutMs);
+        HighGui.destroyAllWindows();
+    }
+
+    public static void showImage(Mat frame, int timeoutMs) {
+        showImage(frame, "", timeoutMs);
+    }
+
+    public static void showImage(Mat frame, String title) {
+        showImage(frame, title, DefaultTimeoutMillis);
+    }
+
+    public static void showImage(Mat frame) {
+        showImage(frame, DefaultTimeoutMillis);
+    }
+}

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

@@ -1,6 +1,5 @@
 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;
@@ -11,9 +10,14 @@ import java.nio.file.attribute.PosixFilePermission;
 import java.util.Arrays;
 import java.util.HashSet;
 import java.util.Set;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 public class FileUtils {
-    private static DebugLogger logger = new DebugLogger(true);
+
+    private FileUtils() {}
+
+    private static Logger logger = LoggerFactory.getLogger(FileUtils.class);
     private static final Set<PosixFilePermission> allReadWriteExecutePerms =
             new HashSet<>(Arrays.asList(PosixFilePermission.values()));
 
@@ -23,7 +27,7 @@ public class FileUtils {
             Set<PosixFilePermission> perms =
                     Files.readAttributes(path, PosixFileAttributes.class).permissions();
             if (!perms.equals(allReadWriteExecutePerms)) {
-                logger.printInfo("Setting perms on" + path.toString());
+                logger.info("Setting perms on" + path.toString());
                 Files.setPosixFilePermissions(path, perms);
                 if (thisFile.isDirectory()) {
                     for (File subfile : thisFile.listFiles()) {
@@ -46,8 +50,7 @@ public class FileUtils {
             }
         } else {
             // TODO file perms on Windows
-            System.out.println(
-                    "File permission setting not available on Windows. Not changing file permissions.");
+            logger.info("Cannot set directory permissions on Windows!");
         }
     }
 }

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

@@ -26,4 +26,8 @@ public class MathUtils {
         double toMult = Math.pow(10, to);
         return (double) Math.round(value * toMult) / toMult;
     }
+
+    public static double nanosToMillis(long nanos) {
+        return nanos / 1000000.0;
+    }
 }

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

@@ -1,5 +1,7 @@
 package com.chameleonvision.common.util.numbers;
 
+import org.opencv.core.Point;
+
 public class DoubleCouple extends NumberCouple<Double> {
 
     public DoubleCouple() {
@@ -9,4 +11,17 @@ public class DoubleCouple extends NumberCouple<Double> {
     public DoubleCouple(Double first, Double second) {
         super(first, second);
     }
+
+    public DoubleCouple(Point point) {
+        super(point.x, point.y);
+    }
+
+    public Point toPoint() {
+        return new Point(first, second);
+    }
+
+    public void fromPoint(Point point) {
+        first = point.x;
+        second = point.y;
+    }
 }

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

@@ -2,8 +2,8 @@ package com.chameleonvision.common.util.numbers;
 
 public abstract class NumberCouple<T extends Number> {
 
-    private T first;
-    private T second;
+    protected T first;
+    protected T second;
 
     public NumberCouple(T first, T second) {
         this.first = first;

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

@@ -1,49 +0,0 @@
-package com.chameleonvision.common.vision.camera;
-
-import edu.wpi.cscore.VideoMode;
-import org.apache.commons.math3.fraction.Fraction;
-import org.apache.commons.math3.util.FastMath;
-import org.opencv.core.Point;
-
-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 Point centerPoint;
-    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 = imageHeight * imageWidth;
-        centerX = imageWidth / 2.0 - 0.5;
-        centerY = imageHeight / 2.0 - 0.5;
-        centerPoint = new Point(centerX, centerY);
-
-        // Calculations from pinhole-model.
-        double diagonalView = FastMath.toRadians(this.fov);
-        Fraction aspectRatio = new Fraction(imageWidth, imageHeight);
-
-        int horizontalRatio = aspectRatio.getNumerator();
-        int verticalRatio = aspectRatio.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 = imageWidth / (2 * FastMath.tan(horizontalView / 2));
-        verticalFocalLength = imageHeight / (2 * FastMath.tan(verticalView / 2));
-    }
-}

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

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

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

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

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

@@ -2,6 +2,4 @@ package com.chameleonvision.common.vision.frame;
 
 public interface FrameProvider {
     Frame getFrame();
-
-    FrameStaticProperties getFrameProperties();
 }

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

@@ -3,6 +3,7 @@ package com.chameleonvision.common.vision.frame;
 import edu.wpi.cscore.VideoMode;
 import org.apache.commons.math3.fraction.Fraction;
 import org.apache.commons.math3.util.FastMath;
+import org.opencv.core.Point;
 
 /** Represents the properties of a frame. */
 public class FrameStaticProperties {
@@ -12,6 +13,7 @@ public class FrameStaticProperties {
     public final double imageArea;
     public final double centerX;
     public final double centerY;
+    public final Point centerPoint;
     public final double horizontalFocalLength;
     public final double verticalFocalLength;
 
@@ -41,6 +43,7 @@ public class FrameStaticProperties {
 
         centerX = ((double) this.imageWidth / 2) - 0.5;
         centerY = ((double) this.imageHeight / 2) - 0.5;
+        centerPoint = new Point(centerX, centerY);
 
         // pinhole model calculations
         double diagonalView = FastMath.toRadians(this.fov);

chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/DummyFrameConsumer.java

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

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

@@ -3,6 +3,7 @@ package com.chameleonvision.common.vision.frame.provider;
 import com.chameleonvision.common.vision.frame.Frame;
 import com.chameleonvision.common.vision.frame.FrameProvider;
 import com.chameleonvision.common.vision.frame.FrameStaticProperties;
+import com.chameleonvision.common.vision.opencv.CVMat;
 import java.nio.file.Files;
 import java.nio.file.Path;
 import java.nio.file.Paths;
@@ -51,7 +52,7 @@ public class FileFrameProvider implements FrameProvider {
 
         if (image.cols() > 0 && image.rows() > 0) {
             m_properties = new FrameStaticProperties(image.width(), image.height(), m_fov);
-            m_frame = new Frame(image);
+            m_frame = new Frame(new CVMat(image), m_properties);
         } else {
             throw new RuntimeException("Image loading failed!");
         }
@@ -76,11 +77,6 @@ public class FileFrameProvider implements FrameProvider {
         return m_reloadImage;
     }
 
-    @Override
-    public FrameStaticProperties getFrameProperties() {
-        return m_properties;
-    }
-
     @Override
     public Frame getFrame() {
         if (m_reloadImage) {

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

@@ -2,7 +2,6 @@ package com.chameleonvision.common.vision.frame.provider;
 
 import com.chameleonvision.common.vision.frame.Frame;
 import com.chameleonvision.common.vision.frame.FrameProvider;
-import com.chameleonvision.common.vision.frame.FrameStaticProperties;
 import org.apache.commons.lang3.NotImplementedException;
 
 public class NetworkFrameProvider implements FrameProvider {
@@ -10,9 +9,4 @@ public class NetworkFrameProvider implements FrameProvider {
     public Frame getFrame() {
         throw new NotImplementedException("");
     }
-
-    @Override
-    public FrameStaticProperties getFrameProperties() {
-        throw new NotImplementedException("");
-    }
 }

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

@@ -2,7 +2,6 @@ package com.chameleonvision.common.vision.frame.provider;
 
 import com.chameleonvision.common.vision.frame.Frame;
 import com.chameleonvision.common.vision.frame.FrameProvider;
-import com.chameleonvision.common.vision.frame.FrameStaticProperties;
 import org.apache.commons.lang3.NotImplementedException;
 
 public class USBFrameProvider implements FrameProvider {
@@ -10,9 +9,4 @@ public class USBFrameProvider implements FrameProvider {
     public Frame getFrame() {
         throw new NotImplementedException("");
     }
-
-    @Override
-    public FrameStaticProperties getFrameProperties() {
-        throw new NotImplementedException("");
-    }
 }

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

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

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

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

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

@@ -1,14 +1,18 @@
 package com.chameleonvision.common.vision.opencv;
 
 import com.chameleonvision.common.util.math.MathUtils;
-import java.util.ArrayList;
 import java.util.Comparator;
-import java.util.List;
-import org.opencv.core.*;
+import org.opencv.core.CvType;
+import org.opencv.core.MatOfInt;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.RotatedRect;
 import org.opencv.imgproc.Imgproc;
 import org.opencv.imgproc.Moments;
 
-public class Contour {
+public class Contour implements Releasable {
 
     public static final Comparator<Contour> SortByMomentsX =
             Comparator.comparingDouble(
@@ -17,14 +21,36 @@ public class Contour {
     public final MatOfPoint mat;
 
     private Double area = Double.NaN;
+    private Double perimeter = Double.NaN;
+    private MatOfPoint2f mat2f = null;
     private RotatedRect minAreaRect = null;
     private Rect boundingRect = null;
     private Moments moments = null;
 
+    private MatOfPoint2f convexHull = null;
+
     public Contour(MatOfPoint mat) {
         this.mat = mat;
     }
 
+    public MatOfPoint2f getMat2f() {
+        if (mat2f == null) {
+            mat2f = new MatOfPoint2f(mat.toArray());
+            mat.convertTo(mat2f, CvType.CV_32F);
+        }
+        return mat2f;
+    }
+
+    public MatOfPoint2f getConvexHull() {
+        if (this.convexHull == null) {
+            var ints = new MatOfInt();
+            Imgproc.convexHull(mat, ints);
+            this.convexHull = Contour.convertIndexesToPoints(mat, ints);
+            ints.release();
+        }
+        return convexHull;
+    }
+
     public double getArea() {
         if (Double.isNaN(area)) {
             area = Imgproc.contourArea(mat);
@@ -32,11 +58,16 @@ public class Contour {
         return area;
     }
 
+    public double getPerimeter() {
+        if (Double.isNaN(perimeter)) {
+            perimeter = Imgproc.arcLength(getMat2f(), true);
+        }
+        return perimeter;
+    }
+
     public RotatedRect getMinAreaRect() {
         if (minAreaRect == null) {
-            MatOfPoint2f temp = new MatOfPoint2f(mat.toArray());
-            minAreaRect = Imgproc.minAreaRect(temp);
-            temp.release();
+            minAreaRect = Imgproc.minAreaRect(getMat2f());
         }
         return minAreaRect;
     }
@@ -60,20 +91,23 @@ public class Contour {
     }
 
     public boolean isEmpty() {
-        return mat.cols() != 0 && mat.rows() != 0;
+        return mat.empty();
     }
 
-    public boolean isIntersecting(Contour secondContour, ContourIntersection intersection) {
+    public boolean isIntersecting(
+            Contour secondContour, ContourIntersectionDirection intersectionDirection) {
         boolean isIntersecting = false;
 
-        if (intersection == ContourIntersection.None) {
+        if (intersectionDirection == ContourIntersectionDirection.None) {
             isIntersecting = true;
         } else {
             try {
                 MatOfPoint2f intersectMatA = new MatOfPoint2f();
                 MatOfPoint2f intersectMatB = new MatOfPoint2f();
-                intersectMatA.fromArray(mat.toArray());
-                intersectMatB.fromArray(secondContour.mat.toArray());
+
+                mat.convertTo(intersectMatA, CvType.CV_32F);
+                secondContour.mat.convertTo(intersectMatB, CvType.CV_32F);
+
                 RotatedRect a = Imgproc.fitEllipse(intersectMatA);
                 RotatedRect b = Imgproc.fitEllipse(intersectMatB);
                 double mA = MathUtils.toSlope(a.angle);
@@ -86,7 +120,7 @@ public class Contour {
                 double intersectionY = (mA * (intersectionX - x0A)) + y0A;
                 double massX = (x0A + x0B) / 2;
                 double massY = (y0A + y0B) / 2;
-                switch (intersection) {
+                switch (intersectionDirection) {
                     case Up:
                         if (intersectionY < massY) isIntersecting = true;
                         break;
@@ -112,47 +146,53 @@ public class Contour {
 
     // TODO: refactor to do "infinite" contours
     public static Contour groupContoursByIntersection(
-            Contour firstContour, Contour secondContour, ContourIntersection intersection) {
-        if (firstContour.isIntersecting(secondContour, intersection)) {
+            Contour firstContour, Contour secondContour, ContourIntersectionDirection intersection) {
+        if (areIntersecting(firstContour, secondContour, intersection)) {
             return combineContours(firstContour, secondContour);
         } else {
             return null;
         }
     }
 
-    // TODO: does this leak?
+    public static boolean areIntersecting(
+            Contour firstContour,
+            Contour secondContour,
+            ContourIntersectionDirection intersectionDirection) {
+        return firstContour.isIntersecting(secondContour, intersectionDirection)
+                || secondContour.isIntersecting(firstContour, intersectionDirection);
+    }
+
     private static Contour combineContours(Contour... contours) {
-        List<Point> fullContourPoints = new ArrayList<>();
+        var points = new MatOfPoint();
 
         for (var contour : contours) {
-            fullContourPoints.addAll(contour.mat.toList());
+            points.push_back(contour.mat);
         }
 
-        var points = new MatOfPoint(fullContourPoints.toArray(new Point[0]));
         var finalContour = new Contour(points);
 
-        if (!finalContour.isEmpty()) {
-            return finalContour;
-        } else return null;
+        boolean contourEmpty = finalContour.isEmpty();
+        return contourEmpty ? null : finalContour;
     }
 
-    // TODO: move these? also docs plox
-    public enum ContourIntersection {
-        None,
-        Up,
-        Down,
-        Left,
-        Right
+    @Override
+    public void release() {
+        mat.release();
+        mat2f.release();
+        convexHull.release();
     }
 
-    public enum ContourGrouping {
-        Single(1),
-        Dual(2);
+    public static MatOfPoint2f convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) {
+        int[] arrIndex = indexes.toArray();
+        Point[] arrContour = contour.toArray();
+        Point[] arrPoints = new Point[arrIndex.length];
 
-        public final int count;
-
-        ContourGrouping(int count) {
-            this.count = count;
+        for (int i = 0; i < arrIndex.length; i++) {
+            arrPoints[i] = arrContour[arrIndex[i]];
         }
+
+        var hull = new MatOfPoint2f();
+        hull.fromArray(arrPoints);
+        return hull;
     }
 }

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

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

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

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

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

@@ -0,0 +1,15 @@
+package com.chameleonvision.common.vision.opencv;
+
+public enum ContourShape {
+    Custom(-1),
+    Circle(0),
+    Triangle(3),
+    Square(4),
+    Rectangle(4);
+
+    public final int sides;
+
+    ContourShape(int sides) {
+        this.sides = sides;
+    }
+}

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

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

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

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

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

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

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

@@ -1,4 +1,4 @@
-package com.chameleonvision.common.vision.pipeline;
+package com.chameleonvision.common.vision.pipe;
 
 import java.util.function.Function;
 
@@ -10,9 +10,9 @@ import java.util.function.Function;
 * @param <O> Output type for the pipe
 * @param <P> Parameters type for the pipe
 */
-public abstract class CVPipe<I, O, P> implements Function<I, PipeResult<O>> {
+public abstract class CVPipe<I, O, P> implements Function<I, CVPipeResult<O>> {
 
-    protected PipeResult<O> result = new PipeResult<>();
+    protected CVPipeResult<O> result = new CVPipeResult<>();
     protected P params;
 
     public void setParams(P params) {
@@ -32,7 +32,7 @@ public abstract class CVPipe<I, O, P> implements Function<I, PipeResult<O>> {
     * @return Result of processing.
     */
     @Override
-    public PipeResult<O> apply(I in) {
+    public CVPipeResult<O> apply(I in) {
         long pipeStartNanos = System.nanoTime();
         result.result = process(in);
         result.nanosElapsed = System.nanoTime() - pipeStartNanos;

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

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

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

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

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

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

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

@@ -1,6 +1,6 @@
-package com.chameleonvision.common.vision.pipeline.pipe;
+package com.chameleonvision.common.vision.pipe.impl;
 
-import com.chameleonvision.common.vision.pipeline.CVPipe;
+import com.chameleonvision.common.vision.pipe.CVPipe;
 import org.opencv.core.Mat;
 import org.opencv.core.Size;
 import org.opencv.imgproc.Imgproc;