OutputMatPipe fixes (#43)

* Fix OutputMatPipe, 3d target list

* Cleanup and reordering, re-add colorspace conversion

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

@@ -19,37 +19,16 @@ package org.photonvision.vision.pipe.impl;
 
 import org.opencv.core.Mat;
 import org.opencv.imgproc.Imgproc;
-import org.photonvision.vision.opencv.DualMat;
 import org.photonvision.vision.pipe.CVPipe;
 
-public class OutputMatPipe extends CVPipe<DualMat, Mat, OutputMatPipe.OutputMatParams> {
-
-    private Mat m_outputMat = new Mat();
+public class OutputMatPipe extends CVPipe<Mat, Mat, OutputMatPipe.OutputMatParams> {
 
     @Override
-    protected Mat process(DualMat in) {
-        Mat rawCam = in.first;
-        Mat hsv = in.second;
-        if (params.showThreshold()) {
-            // convert input mat
-            hsv.copyTo(m_outputMat);
-            Imgproc.cvtColor(m_outputMat, m_outputMat, Imgproc.COLOR_GRAY2BGR, 3);
-        } else {
-            m_outputMat = rawCam;
-        }
-
-        return m_outputMat;
+    protected Mat process(Mat in) {
+        // convert input mat
+        Imgproc.cvtColor(in, in, Imgproc.COLOR_GRAY2BGR, 3);
+        return in;
     }
 
-    public static class OutputMatParams {
-        private final boolean m_showThreshold;
-
-        public OutputMatParams(boolean showThreshold) {
-            m_showThreshold = showThreshold;
-        }
-
-        public boolean showThreshold() {
-            return m_showThreshold;
-        }
-    }
+    public static class OutputMatParams {}
 }

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

@@ -33,6 +33,7 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.PotentialTarget;
 import org.photonvision.vision.target.TrackedTarget;
 
+@SuppressWarnings({"DuplicatedCode", "UnusedAssignment"})
 public class ColoredShapePipeline
         extends CVPipeline<CVPipelineResult, ColoredShapePipelineSettings> {
 
@@ -56,9 +57,6 @@ public class ColoredShapePipeline
 
     private final Mat rawInputMat = new Mat();
     private final DualMat outputMats = new DualMat();
-    private List<CVShape> shapes;
-    private CVPipeResult<Mat> result;
-    private CVPipeResult<List<TrackedTarget>> targetList;
     private final Point[] rectPoints = new Point[4];
 
     public ColoredShapePipeline() {
@@ -86,8 +84,7 @@ public class ColoredShapePipeline
                 new HSVPipe.HSVParams(settings.hsvHue, settings.hsvSaturation, settings.hsvValue);
         hsvPipe.setParams(hsvParams);
 
-        OutputMatPipe.OutputMatParams outputMatParams =
-                new OutputMatPipe.OutputMatParams(settings.outputShowThresholded);
+        OutputMatPipe.OutputMatParams outputMatParams = new OutputMatPipe.OutputMatParams();
         outputMatPipe.setParams(outputMatParams);
 
         SpeckleRejectPipe.SpeckleRejectParams speckleRejectParams =
@@ -183,23 +180,22 @@ public class ColoredShapePipeline
 
         long sumPipeNanosElapsed = 0L;
 
-        frame.image.getMat().copyTo(rawInputMat);
-
         CVPipeResult<Mat> rotateImageResult = rotateImagePipe.apply(frame.image.getMat());
         sumPipeNanosElapsed += rotateImageResult.nanosElapsed;
 
+        rawInputMat.release();
+        frame.image.getMat().copyTo(rawInputMat);
+
         CVPipeResult<Mat> erodeDilateResult = erodeDilatePipe.apply(rotateImageResult.result);
         sumPipeNanosElapsed += erodeDilateResult.nanosElapsed;
 
         CVPipeResult<Mat> hsvPipeResult = hsvPipe.apply(erodeDilateResult.result);
         sumPipeNanosElapsed += hsvPipeResult.nanosElapsed;
 
+        // the first is the raw input mat, the second is the HSVPipe result
         outputMats.first = rawInputMat;
         outputMats.second = hsvPipeResult.result;
 
-        CVPipeResult<Mat> outputMatResult = outputMatPipe.apply(outputMats);
-        sumPipeNanosElapsed += outputMatResult.nanosElapsed;
-
         CVPipeResult<List<Contour>> findContoursResult = findContoursPipe.apply(hsvPipeResult.result);
         sumPipeNanosElapsed += findContoursResult.nanosElapsed;
 
@@ -207,6 +203,7 @@ public class ColoredShapePipeline
                 speckleRejectPipe.apply(findContoursResult.result);
         sumPipeNanosElapsed += speckleRejectResult.nanosElapsed;
 
+        List<CVShape> shapes;
         if (settings.desiredShape == ContourShape.Circle) {
             CVPipeResult<List<CVShape>> findCirclesResult =
                     findCirclesPipe.apply(Pair.of(hsvPipeResult.result, speckleRejectResult.result));
@@ -237,6 +234,8 @@ public class ColoredShapePipeline
                 collect2dTargetsPipe.apply(sortContoursResult.result);
         sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed;
 
+        List<TrackedTarget> targetList;
+
         if (settings.solvePNPEnabled && settings.desiredShape == ContourShape.Circle) {
             var cornerDetectionResult = cornerDetectionPipe.apply(collect2dTargetsResult.result);
             collect2dTargetsResult.result.forEach(
@@ -249,32 +248,55 @@ public class ColoredShapePipeline
             var solvePNPResult = solvePNPPipe.apply(cornerDetectionResult.result);
             sumPipeNanosElapsed += solvePNPResult.nanosElapsed;
 
-            targetList = solvePNPResult;
+            targetList = solvePNPResult.result;
         } else {
-            targetList = collect2dTargetsResult;
+            targetList = collect2dTargetsResult.result;
         }
 
-        CVPipeResult<Mat> draw2dCrosshairResult =
-                draw2dCrosshairPipe.apply(Pair.of(outputMatResult.result, targetList.result));
-        sumPipeNanosElapsed += draw2dCrosshairResult.nanosElapsed;
+        // the first is the raw input mat, the second is the HSVPipe result
+        CVPipeResult<Mat> drawOnInputResult, drawOnOutputResult;
 
-        CVPipeResult<Mat> draw2dContoursResult =
+        CVPipeResult<Mat> draw2dCrosshairResultOnInput =
+                draw2dCrosshairPipe.apply(Pair.of(outputMats.first, targetList));
+        sumPipeNanosElapsed += draw2dCrosshairResultOnInput.nanosElapsed;
+
+        CVPipeResult<Mat> draw2dCrosshairResultOnOutput =
+                draw2dCrosshairPipe.apply(Pair.of(outputMats.second, targetList));
+        sumPipeNanosElapsed += draw2dCrosshairResultOnOutput.nanosElapsed;
+
+        CVPipeResult<Mat> draw2dContoursResultOnInput =
                 draw2DTargetsPipe.apply(
-                        Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
-        sumPipeNanosElapsed += draw2dContoursResult.nanosElapsed;
+                        Pair.of(draw2dCrosshairResultOnInput.result, collect2dTargetsResult.result));
+        sumPipeNanosElapsed += draw2dContoursResultOnInput.nanosElapsed;
+
+        CVPipeResult<Mat> draw2dContoursResultOnOutput =
+                draw2DTargetsPipe.apply(
+                        Pair.of(draw2dCrosshairResultOnOutput.result, collect2dTargetsResult.result));
+        sumPipeNanosElapsed += draw2dContoursResultOnOutput.nanosElapsed;
 
         if (settings.solvePNPEnabled && settings.desiredShape == ContourShape.Circle) {
-            result =
+            drawOnInputResult =
                     draw3dTargetsPipe.apply(
-                            Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
-            sumPipeNanosElapsed += result.nanosElapsed;
+                            Pair.of(draw2dContoursResultOnInput.result, collect2dTargetsResult.result));
+            sumPipeNanosElapsed += drawOnInputResult.nanosElapsed;
+
+            drawOnOutputResult =
+                    draw3dTargetsPipe.apply(
+                            Pair.of(draw2dContoursResultOnOutput.result, collect2dTargetsResult.result));
+            sumPipeNanosElapsed += drawOnOutputResult.nanosElapsed;
         } else {
-            result = draw2dContoursResult;
+            drawOnInputResult = draw2dContoursResultOnInput;
+            drawOnOutputResult = draw2dContoursResultOnOutput;
         }
 
+        // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
+        CVPipeResult<Mat> outputMatPipeResult = outputMatPipe.apply(outputMats.second);
+        sumPipeNanosElapsed += outputMatPipeResult.nanosElapsed;
+
         return new CVPipelineResult(
                 MathUtils.nanosToMillis(sumPipeNanosElapsed),
-                collect2dTargetsResult.result,
-                new Frame(new CVMat(result.result), frame.frameStaticProperties));
+                targetList,
+                new Frame(new CVMat(outputMats.second), frame.frameStaticProperties),
+                new Frame(new CVMat(outputMats.first), frame.frameStaticProperties));
     }
 }

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

@@ -20,7 +20,6 @@ package org.photonvision.vision.pipeline;
 import java.util.List;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.Mat;
-import org.opencv.imgproc.Imgproc;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameStaticProperties;
@@ -48,6 +47,7 @@ import org.photonvision.vision.target.PotentialTarget;
 import org.photonvision.vision.target.TrackedTarget;
 
 /** Represents a pipeline for tracking retro-reflective targets. */
+@SuppressWarnings({"UnusedAssignment", "DuplicatedCode"})
 public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectivePipelineSettings> {
 
     private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
@@ -66,8 +66,8 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
     private final Draw2dTargetsPipe draw2DTargetsPipe = new Draw2dTargetsPipe();
     private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
 
-    private Mat rawInputMat = new Mat();
-    private DualMat outputMats = new DualMat();
+    private final Mat rawInputMat = new Mat();
+    private final DualMat outputMats = new DualMat();
 
     public ReflectivePipeline() {
         settings = new ReflectivePipelineSettings();
@@ -161,7 +161,6 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
         solvePNPPipe.setParams(solvePNPParams);
     }
 
-    @SuppressWarnings("DuplicatedCode")
     @Override
     public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings) {
         setPipeParams(frame.frameStaticProperties, settings);
@@ -180,8 +179,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
         CVPipeResult<Mat> hsvPipeResult = hsvPipe.apply(erodeDilateResult.result);
         sumPipeNanosElapsed += hsvPipeResult.nanosElapsed;
 
-        // mat leak fix attempt
-        // the first is the raw input mat, the second is the hsvpipe result
+        // the first is the raw input mat, the second is the HSVPipe result
         outputMats.first = rawInputMat;
         outputMats.second = hsvPipeResult.result;
 
@@ -208,7 +206,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
                 collect2dTargetsPipe.apply(sortContoursResult.result);
         sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed;
 
-        CVPipeResult<List<TrackedTarget>> targetList;
+        List<TrackedTarget> targetList;
 
         // 3d stuff
         if (settings.solvePNPEnabled) {
@@ -218,58 +216,57 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
             var solvePNPResult = solvePNPPipe.apply(cornerDetectionResult.result);
             sumPipeNanosElapsed += solvePNPResult.nanosElapsed;
 
-            targetList = solvePNPResult;
+            targetList = solvePNPResult.result;
         } else {
-            targetList = collect2dTargetsResult;
+            targetList = collect2dTargetsResult.result;
         }
 
+        // the first is the raw input mat, the second is the HSVPipe result
         CVPipeResult<Mat> drawOnInputResult, drawOnOutputResult;
 
-        // the first is the raw input mat, the second is the hsvpipe result
-        // Draw on input
-
+        // Draw 2D Crosshair on input and output
         CVPipeResult<Mat> draw2dCrosshairResultOnInput =
-                draw2dCrosshairPipe.apply(Pair.of(outputMats.first, targetList.result));
+                draw2dCrosshairPipe.apply(Pair.of(outputMats.first, targetList));
         sumPipeNanosElapsed += draw2dCrosshairResultOnInput.nanosElapsed;
 
+        CVPipeResult<Mat> draw2dCrosshairResultOnOutput =
+                draw2dCrosshairPipe.apply(Pair.of(outputMats.second, targetList));
+        sumPipeNanosElapsed += draw2dCrosshairResultOnOutput.nanosElapsed;
+
+        // Draw 2D contours on input and output
         CVPipeResult<Mat> draw2dContoursResultOnInput =
                 draw2DTargetsPipe.apply(
                         Pair.of(draw2dCrosshairResultOnInput.result, collect2dTargetsResult.result));
         sumPipeNanosElapsed += draw2dCrosshairResultOnInput.nanosElapsed;
 
-        if (settings.solvePNPEnabled) {
-            drawOnInputResult =
-                    draw3dTargetsPipe.apply(
-                            Pair.of(draw2dContoursResultOnInput.result, collect2dTargetsResult.result));
-            sumPipeNanosElapsed += drawOnInputResult.nanosElapsed;
-        } else {
-            drawOnInputResult = draw2dContoursResultOnInput;
-        }
-
-        // Draw on output
-
-        Imgproc.cvtColor(outputMats.second, outputMats.second, Imgproc.COLOR_GRAY2BGR, 3);
-        CVPipeResult<Mat> draw2dCrosshairResultOnOutput =
-                draw2dCrosshairPipe.apply(Pair.of(outputMats.second, targetList.result));
-        sumPipeNanosElapsed += draw2dCrosshairResultOnOutput.nanosElapsed;
-
         CVPipeResult<Mat> draw2dContoursResultOnOutput =
                 draw2DTargetsPipe.apply(
                         Pair.of(draw2dCrosshairResultOnOutput.result, collect2dTargetsResult.result));
         sumPipeNanosElapsed += draw2dContoursResultOnOutput.nanosElapsed;
 
+        // Draw 3D Targets on input and output if necessary
         if (settings.solvePNPEnabled) {
+            drawOnInputResult =
+                    draw3dTargetsPipe.apply(
+                            Pair.of(draw2dContoursResultOnInput.result, collect2dTargetsResult.result));
+            sumPipeNanosElapsed += drawOnInputResult.nanosElapsed;
+
             drawOnOutputResult =
                     draw3dTargetsPipe.apply(
                             Pair.of(draw2dContoursResultOnOutput.result, collect2dTargetsResult.result));
             sumPipeNanosElapsed += drawOnOutputResult.nanosElapsed;
         } else {
+            drawOnInputResult = draw2dContoursResultOnInput;
             drawOnOutputResult = draw2dContoursResultOnOutput;
         }
 
+        // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
+        CVPipeResult<Mat> outputMatPipeResult = outputMatPipe.apply(outputMats.second);
+        sumPipeNanosElapsed += outputMatPipeResult.nanosElapsed;
+
         return new CVPipelineResult(
                 MathUtils.nanosToMillis(sumPipeNanosElapsed),
-                collect2dTargetsResult.result,
+                targetList,
                 new Frame(new CVMat(outputMats.second), frame.frameStaticProperties),
                 new Frame(new CVMat(outputMats.first), frame.frameStaticProperties));
     }