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>

chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineTest.java

@@ -0,0 +1,125 @@
+package com.chameleonvision.common.vision.pipeline;
+
+import com.chameleonvision.common.util.TestUtils;
+import com.chameleonvision.common.vision.frame.Frame;
+import com.chameleonvision.common.vision.frame.provider.FileFrameProvider;
+import com.chameleonvision.common.vision.opencv.CVMat;
+import com.chameleonvision.common.vision.opencv.ContourGroupingMode;
+import com.chameleonvision.common.vision.opencv.ContourIntersectionDirection;
+import org.junit.jupiter.api.Assertions;
+import org.junit.jupiter.api.Test;
+import org.slf4j.LoggerFactory;
+
+public class ReflectivePipelineTest {
+
+    public static void setLoggingLevel(ch.qos.logback.classic.Level level) {
+        ch.qos.logback.classic.Logger root =
+                (ch.qos.logback.classic.Logger)
+                        org.slf4j.LoggerFactory.getLogger(ch.qos.logback.classic.Logger.ROOT_LOGGER_NAME);
+        root.setLevel(level);
+    }
+
+    @Test
+    public void testDebug() {
+        var logger = LoggerFactory.getLogger(ReflectivePipelineTest.class);
+        setLoggingLevel(ch.qos.logback.classic.Level.WARN);
+        logger.warn(String.valueOf(logger.isDebugEnabled()));
+        logger.info("hi");
+        logger.debug("debug");
+    }
+
+    @Test
+    public void test2019() {
+        TestUtils.loadLibraries();
+        var pipeline = new ReflectivePipeline();
+
+        var settings = new ReflectivePipelineSettings();
+        settings.hsvHue.set(60, 100);
+        settings.hsvSaturation.set(100, 255);
+        settings.hsvValue.set(190, 255);
+        settings.outputShowThresholded = true;
+        settings.outputShowMultipleTargets = true;
+        settings.contourGroupingMode = ContourGroupingMode.Dual;
+        settings.contourIntersection = ContourIntersectionDirection.Up;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes),
+                        TestUtils.WPI2019Image.FOV);
+
+        TestUtils.showImage(frameProvider.getFrame().image.getMat(), "Pipeline input", 1);
+
+        CVPipelineResult pipelineResult;
+
+        pipelineResult = pipeline.run(frameProvider.getFrame(), settings);
+        printTestResults(pipelineResult);
+
+        Assertions.assertTrue(pipelineResult.hasTargets());
+        Assertions.assertEquals(2, pipelineResult.targets.size());
+
+        TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output");
+    }
+
+    @Test
+    public void test2020() {
+        TestUtils.loadLibraries();
+        var pipeline = new ReflectivePipeline();
+
+        var settings = new ReflectivePipelineSettings();
+        settings.hsvHue.set(60, 100);
+        settings.hsvSaturation.set(200, 255);
+        settings.hsvValue.set(200, 255);
+        settings.outputShowThresholded = true;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center),
+                        TestUtils.WPI2020Image.FOV);
+
+        CVPipelineResult pipelineResult = pipeline.run(frameProvider.getFrame(), settings);
+        printTestResults(pipelineResult);
+
+        TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output");
+    }
+
+    private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) {
+        var pipeline = new ReflectivePipeline();
+
+        while (true) {
+            CVPipelineResult pipelineResult = pipeline.run(frame, settings);
+            printTestResults(pipelineResult);
+            int preRelease = CVMat.getMatCount();
+            pipelineResult.release();
+            int postRelease = CVMat.getMatCount();
+
+            System.out.printf("Pre: %d, Post: %d\n", preRelease, postRelease);
+        }
+    }
+
+    // used to run VisualVM for profiling. It won't run on unit tests.
+    public static void main(String[] args) {
+        TestUtils.loadLibraries();
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes),
+                        TestUtils.WPI2019Image.FOV);
+
+        var settings = new ReflectivePipelineSettings();
+        settings.hsvHue.set(60, 100);
+        settings.hsvSaturation.set(100, 255);
+        settings.hsvValue.set(190, 255);
+        settings.outputShowThresholded = true;
+        settings.outputShowMultipleTargets = true;
+        settings.contourGroupingMode = ContourGroupingMode.Dual;
+        settings.contourIntersection = ContourIntersectionDirection.Up;
+
+        continuouslyRunPipeline(frameProvider.getFrame(), settings);
+    }
+
+    private static void printTestResults(CVPipelineResult pipelineResult) {
+        double fps = 1000 / pipelineResult.getLatencyMillis();
+        System.out.print(
+                "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " fps), ");
+        System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
+    }
+}

chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/SolvePNPTest.java

@@ -0,0 +1,197 @@
+package com.chameleonvision.common.vision.pipeline;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+
+import com.chameleonvision.common.calibration.CameraCalibrationCoefficients;
+import com.chameleonvision.common.util.TestUtils;
+import com.chameleonvision.common.vision.frame.Frame;
+import com.chameleonvision.common.vision.frame.provider.FileFrameProvider;
+import com.chameleonvision.common.vision.opencv.CVMat;
+import com.chameleonvision.common.vision.opencv.ContourGroupingMode;
+import com.chameleonvision.common.vision.opencv.ContourIntersectionDirection;
+import com.chameleonvision.common.vision.target.TargetModel;
+import com.chameleonvision.common.vision.target.TrackedTarget;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.util.stream.Collectors;
+import org.junit.jupiter.api.Test;
+
+public class SolvePNPTest {
+
+    @Test
+    public void meme() throws IOException {
+        TestUtils.loadLibraries();
+
+        var lowres = (Path.of(TestUtils.getCalibrationPath().toString(), "lifecamcal.json").toFile());
+        var cal1 = new ObjectMapper().readValue(lowres, CameraCalibrationCoefficients.class);
+
+        var highres = (Path.of(TestUtils.getCalibrationPath().toString(), "lifecamcal2.json").toFile());
+        var cal2 = new ObjectMapper().readValue(highres, CameraCalibrationCoefficients.class);
+    }
+
+    private CameraCalibrationCoefficients get640p() {
+        try {
+            var cameraCalibration =
+                    new ObjectMapper()
+                            .readValue(
+                                    (Path.of(TestUtils.getCalibrationPath().toString(), "lifecam640p.json").toFile()),
+                                    CameraCalibrationCoefficients.class);
+
+            assertEquals(3, cameraCalibration.cameraIntrinsics.rows);
+            assertEquals(3, cameraCalibration.cameraIntrinsics.cols);
+            assertEquals(1, cameraCalibration.cameraExtrinsics.rows);
+            assertEquals(5, cameraCalibration.cameraExtrinsics.cols);
+            assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows());
+            assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().cols());
+            assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMat().rows());
+            assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMat().cols());
+            assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows());
+            assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols());
+            assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().rows());
+            assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().cols());
+            assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().rows());
+            assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().cols());
+            assertEquals(1, cameraCalibration.getCameraExtrinsicsMat().rows());
+            assertEquals(5, cameraCalibration.getCameraExtrinsicsMat().cols());
+
+            return cameraCalibration;
+        } catch (IOException e) {
+            e.printStackTrace();
+            return null;
+        }
+    }
+
+    @Test
+    public void test2019() {
+        TestUtils.loadLibraries();
+        var pipeline = new ReflectivePipeline();
+
+        var settings = new ReflectivePipelineSettings();
+        settings.hsvHue.set(60, 100);
+        settings.hsvSaturation.set(100, 255);
+        settings.hsvValue.set(190, 255);
+        settings.outputShowThresholded = true;
+        settings.outputShowMultipleTargets = true;
+        settings.solvePNPEnabled = true;
+        settings.contourGroupingMode = ContourGroupingMode.Dual;
+        settings.contourIntersection = ContourIntersectionDirection.Up;
+        settings.cornerDetectionUseConvexHulls = true;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark48in),
+                        TestUtils.WPI2019Image.FOV);
+
+        CVPipelineResult pipelineResult;
+
+        pipelineResult = pipeline.run(frameProvider.getFrame(), settings);
+
+        TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 1000 * 90);
+    }
+
+    @Test
+    public void test2020() {
+        TestUtils.loadLibraries();
+        var pipeline = new ReflectivePipeline();
+
+        var settings = new ReflectivePipelineSettings();
+        settings.hsvHue.set(60, 100);
+        settings.hsvSaturation.set(100, 255);
+        settings.hsvValue.set(60, 255);
+        settings.outputShowThresholded = true;
+        settings.solvePNPEnabled = true;
+        settings.cornerDetectionAccuracyPercentage = 4;
+        settings.cornerDetectionUseConvexHulls = true;
+        settings.cameraCalibration = get640p();
+        settings.targetModel = TargetModel.get2020Target(36);
+        settings.cameraPitch = Rotation2d.fromDegrees(0.0);
+
+        assertNotNull(settings.cameraCalibration);
+        assertEquals(3, settings.cameraCalibration.cameraIntrinsics.rows);
+        assertEquals(3, settings.cameraCalibration.cameraIntrinsics.cols);
+        assertEquals(1, settings.cameraCalibration.cameraExtrinsics.rows);
+        assertEquals(5, settings.cameraCalibration.cameraExtrinsics.cols);
+
+        assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMat().rows());
+        assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMat().cols());
+        assertEquals(1, settings.cameraCalibration.cameraExtrinsics.getAsMat().rows());
+        assertEquals(5, settings.cameraCalibration.cameraExtrinsics.getAsMat().cols());
+
+        assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows());
+        assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols());
+        assertEquals(1, settings.cameraCalibration.cameraExtrinsics.getAsMatOfDouble().rows());
+        assertEquals(5, settings.cameraCalibration.cameraExtrinsics.getAsMatOfDouble().cols());
+
+        assertEquals(3, settings.cameraCalibration.getCameraIntrinsicsMat().rows());
+        assertEquals(3, settings.cameraCalibration.getCameraIntrinsicsMat().cols());
+        assertEquals(1, settings.cameraCalibration.getCameraExtrinsicsMat().rows());
+        assertEquals(5, settings.cameraCalibration.getCameraExtrinsicsMat().cols());
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_224in_Left),
+                        TestUtils.WPI2020Image.FOV);
+
+        //        TestUtils.showImage(frameProvider.getFrame().image.getMat(), "Pipeline output",
+        // 999999);
+
+        CVPipelineResult pipelineResult = pipeline.run(frameProvider.getFrame(), settings);
+        printTestResults(pipelineResult);
+
+        var pose = pipelineResult.targets.get(0).getRobotRelativePose();
+        //        assertEquals(180, pose.getTranslation().getX(), 20);
+        //        assertEquals(0, pose.getTranslation().getY(), 20);
+        //        assertEquals(0, pose.getRotation().getDegrees(), 5);
+
+        TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999);
+    }
+
+    private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) {
+        var pipeline = new ReflectivePipeline();
+
+        while (true) {
+            CVPipelineResult pipelineResult = pipeline.run(frame, settings);
+            printTestResults(pipelineResult);
+            int preRelease = CVMat.getMatCount();
+            pipelineResult.release();
+            int postRelease = CVMat.getMatCount();
+
+            System.out.printf("Pre: %d, Post: %d\n", preRelease, postRelease);
+        }
+    }
+
+    // used to run VisualVM for profiling. It won't run on unit tests.
+    public static void main(String[] args) {
+        TestUtils.loadLibraries();
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes),
+                        TestUtils.WPI2019Image.FOV);
+
+        var settings = new ReflectivePipelineSettings();
+        settings.hsvHue.set(60, 100);
+        settings.hsvSaturation.set(100, 255);
+        settings.hsvValue.set(190, 255);
+        settings.outputShowThresholded = true;
+        settings.outputShowMultipleTargets = true;
+        settings.contourGroupingMode = ContourGroupingMode.Dual;
+        settings.contourIntersection = ContourIntersectionDirection.Up;
+
+        continuouslyRunPipeline(frameProvider.getFrame(), settings);
+    }
+
+    private static void printTestResults(CVPipelineResult pipelineResult) {
+        double fps = 1000 / pipelineResult.getLatencyMillis();
+        System.out.println(
+                "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)");
+        System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
+        System.out.println(
+                "Found targets at "
+                        + pipelineResult.targets.stream()
+                                .map(TrackedTarget::getRobotRelativePose)
+                                .collect(Collectors.toList()));
+    }
+}

chameleon-server/src/test/resources/calibration/lifecam320p.json

@@ -0,0 +1,34 @@
+{
+  "resolution": {
+    "width": 320.0,
+    "height": 240.0
+  },
+  "cameraIntrinsics": {
+    "rows": 3,
+    "cols": 3,
+    "type": 6,
+    "data": [
+      353.74653217742724,
+      0.0,
+      163.55407989211918,
+      0.0,
+      340.77624878700817,
+      119.8945718300403,
+      0.0,
+      0.0,
+      1.0
+    ]
+  },
+  "cameraExtrinsics": {
+    "rows": 1,
+    "cols": 5,
+    "type": 6,
+    "data": [
+      0.10322037759535845,
+      -0.2890556437050186,
+      0.00406400648501475,
+      2.5573586808275763E-4,
+      -1.462385758978924
+    ]
+  }
+}

chameleon-server/src/test/resources/calibration/lifecam640p.json

@@ -0,0 +1,34 @@
+{
+  "resolution": {
+    "width": 640.0,
+    "height": 480.0
+  },
+  "cameraIntrinsics": {
+    "rows": 3,
+    "cols": 3,
+    "type": 6,
+    "data": [
+      699.3778103158814,
+      0.0,
+      345.6059345433618,
+      0.0,
+      677.7161226393544,
+      207.12741326228522,
+      0.0,
+      0.0,
+      1.0
+    ]
+  },
+  "cameraExtrinsics": {
+    "rows": 1,
+    "cols": 5,
+    "type": 6,
+    "data": [
+      0.14382207979312617,
+      -0.9851192814987014,
+      -0.018168751047242335,
+      0.011034504043795105,
+      1.9833437176538498
+    ]
+  }
+}

chameleon-server/src/test/resources/testimages/2019/WPI/info.txt

@@ -0,0 +1,3 @@
+Taken on Microsoft LifeCam HD-3000
+FOV = 68.5
+Credit to WPILib for these images.

chameleon-server/src/test/resources/testimages/2020/WPI/info.txt

@@ -0,0 +1,3 @@
+Taken on Microsoft LifeCam HD-3000
+FOV = 68.5
+Credit to WPILib for these images.