From 7c35f3b975f35d2337a94b1126d9f32d05d33b7d Mon Sep 17 00:00:00 2001 From: Banks T Date: Sat, 18 Jul 2020 02:28:08 -0400 Subject: [PATCH] OutputMatPipe fixes (#43) * Fix OutputMatPipe, 3d target list * Cleanup and reordering, re-add colorspace conversion --- .../vision/pipe/impl/OutputMatPipe.java | 33 ++------- .../vision/pipeline/ColoredShapePipeline.java | 70 ++++++++++++------- .../vision/pipeline/ReflectivePipeline.java | 57 +++++++-------- 3 files changed, 79 insertions(+), 81 deletions(-) diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/OutputMatPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/OutputMatPipe.java index 3136dadf1..0bb14557c 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/OutputMatPipe.java +++ b/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 { - - private Mat m_outputMat = new Mat(); +public class OutputMatPipe extends CVPipe { @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 {} } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java index 27889d259..e6bfa59b7 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java +++ b/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 { @@ -56,9 +57,6 @@ public class ColoredShapePipeline private final Mat rawInputMat = new Mat(); private final DualMat outputMats = new DualMat(); - private List shapes; - private CVPipeResult result; - private CVPipeResult> 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 rotateImageResult = rotateImagePipe.apply(frame.image.getMat()); sumPipeNanosElapsed += rotateImageResult.nanosElapsed; + rawInputMat.release(); + frame.image.getMat().copyTo(rawInputMat); + CVPipeResult erodeDilateResult = erodeDilatePipe.apply(rotateImageResult.result); sumPipeNanosElapsed += erodeDilateResult.nanosElapsed; CVPipeResult 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 outputMatResult = outputMatPipe.apply(outputMats); - sumPipeNanosElapsed += outputMatResult.nanosElapsed; - CVPipeResult> findContoursResult = findContoursPipe.apply(hsvPipeResult.result); sumPipeNanosElapsed += findContoursResult.nanosElapsed; @@ -207,6 +203,7 @@ public class ColoredShapePipeline speckleRejectPipe.apply(findContoursResult.result); sumPipeNanosElapsed += speckleRejectResult.nanosElapsed; + List shapes; if (settings.desiredShape == ContourShape.Circle) { CVPipeResult> findCirclesResult = findCirclesPipe.apply(Pair.of(hsvPipeResult.result, speckleRejectResult.result)); @@ -237,6 +234,8 @@ public class ColoredShapePipeline collect2dTargetsPipe.apply(sortContoursResult.result); sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed; + List 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 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 drawOnInputResult, drawOnOutputResult; - CVPipeResult draw2dContoursResult = + CVPipeResult draw2dCrosshairResultOnInput = + draw2dCrosshairPipe.apply(Pair.of(outputMats.first, targetList)); + sumPipeNanosElapsed += draw2dCrosshairResultOnInput.nanosElapsed; + + CVPipeResult draw2dCrosshairResultOnOutput = + draw2dCrosshairPipe.apply(Pair.of(outputMats.second, targetList)); + sumPipeNanosElapsed += draw2dCrosshairResultOnOutput.nanosElapsed; + + CVPipeResult draw2dContoursResultOnInput = draw2DTargetsPipe.apply( - Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result)); - sumPipeNanosElapsed += draw2dContoursResult.nanosElapsed; + Pair.of(draw2dCrosshairResultOnInput.result, collect2dTargetsResult.result)); + sumPipeNanosElapsed += draw2dContoursResultOnInput.nanosElapsed; + + CVPipeResult 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 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)); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java index 302fdc289..384d461b3 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java +++ b/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 { private final RotateImagePipe rotateImagePipe = new RotateImagePipe(); @@ -66,8 +66,8 @@ public class ReflectivePipeline extends CVPipeline 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> targetList; + List targetList; // 3d stuff if (settings.solvePNPEnabled) { @@ -218,58 +216,57 @@ public class ReflectivePipeline extends CVPipeline 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 draw2dCrosshairResultOnInput = - draw2dCrosshairPipe.apply(Pair.of(outputMats.first, targetList.result)); + draw2dCrosshairPipe.apply(Pair.of(outputMats.first, targetList)); sumPipeNanosElapsed += draw2dCrosshairResultOnInput.nanosElapsed; + CVPipeResult draw2dCrosshairResultOnOutput = + draw2dCrosshairPipe.apply(Pair.of(outputMats.second, targetList)); + sumPipeNanosElapsed += draw2dCrosshairResultOnOutput.nanosElapsed; + + // Draw 2D contours on input and output CVPipeResult 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 draw2dCrosshairResultOnOutput = - draw2dCrosshairPipe.apply(Pair.of(outputMats.second, targetList.result)); - sumPipeNanosElapsed += draw2dCrosshairResultOnOutput.nanosElapsed; - CVPipeResult 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 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)); }