diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/CVPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/CVPipe.java index ad0a56c9d..7dbd7cb20 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/CVPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/CVPipe.java @@ -17,8 +17,6 @@ package org.photonvision.vision.pipe; -import java.util.function.Function; - /** * Defines a pipe. A pipe is a single step in a pipeline. This class is to be extended, never used * on its own. @@ -27,7 +25,7 @@ import java.util.function.Function; * @param Output type for the pipe * @param

Parameters type for the pipe */ -public abstract class CVPipe implements Function> { +public abstract class CVPipe { protected CVPipeResult result = new CVPipeResult<>(); protected P params; @@ -48,11 +46,15 @@ public abstract class CVPipe implements Function> { * @param in Input for pipe processing. * @return Result of processing. */ - @Override - public CVPipeResult apply(I in) { + public CVPipeResult run(I in) { long pipeStartNanos = System.nanoTime(); - result.result = process(in); + result.output = process(in); result.nanosElapsed = System.nanoTime() - pipeStartNanos; return result; } + + public static class CVPipeResult { + public O output; + public long nanosElapsed; + } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/CVPipeResult.java b/photon-server/src/main/java/org/photonvision/vision/pipe/MutatingPipe.java similarity index 89% rename from photon-server/src/main/java/org/photonvision/vision/pipe/CVPipeResult.java rename to photon-server/src/main/java/org/photonvision/vision/pipe/MutatingPipe.java index 6537e3d5e..97f0872f3 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/CVPipeResult.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/MutatingPipe.java @@ -17,7 +17,4 @@ package org.photonvision.vision.pipe; -public class CVPipeResult { - public O result; - public long nanosElapsed; -} +public abstract class MutatingPipe extends CVPipe {} diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/BlurPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/BlurPipe.java index 115218739..75347fc47 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/BlurPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/BlurPipe.java @@ -20,10 +20,10 @@ package org.photonvision.vision.pipe.impl; import org.opencv.core.Mat; import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; -import org.photonvision.vision.pipe.CVPipe; +import org.photonvision.vision.pipe.MutatingPipe; /** Represents a pipeline that blurs the image. */ -public class BlurPipe extends CVPipe { +public class BlurPipe extends MutatingPipe { /** * Processes this pipe. * @@ -31,9 +31,9 @@ public class BlurPipe extends CVPipe { * @return The processed frame. */ @Override - protected Mat process(Mat in) { + protected Void process(Mat in) { Imgproc.blur(in, in, params.getBlurSize()); - return in; + return null; } public static class BlurParams { diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java index 740b4ef24..b481019ac 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java @@ -25,15 +25,16 @@ import org.opencv.core.Point; import org.opencv.imgproc.Imgproc; import org.photonvision.common.util.ColorHelper; import org.photonvision.common.util.numbers.DoubleCouple; -import org.photonvision.vision.pipe.CVPipe; +import org.photonvision.vision.pipe.MutatingPipe; import org.photonvision.vision.target.RobotOffsetPointMode; import org.photonvision.vision.target.TrackedTarget; public class Draw2dCrosshairPipe - extends CVPipe>, Mat, Draw2dCrosshairPipe.Draw2dCrosshairParams> { + extends MutatingPipe< + Pair>, Draw2dCrosshairPipe.Draw2dCrosshairParams> { @Override - protected Mat process(Pair> in) { + protected Void process(Pair> in) { Mat image = in.getLeft(); if (params.m_showCrosshair) { @@ -61,7 +62,7 @@ public class Draw2dCrosshairPipe Imgproc.line(image, xMax, xMin, ColorHelper.colorToScalar(params.m_crosshairColor)); Imgproc.line(image, yMax, yMin, ColorHelper.colorToScalar(params.m_crosshairColor)); } - return image; + return null; } public static class Draw2dCrosshairParams { diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java index 9a6a1b0e3..a427f6452 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java @@ -24,16 +24,16 @@ import org.apache.commons.lang3.tuple.Pair; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; import org.photonvision.common.util.ColorHelper; -import org.photonvision.vision.pipe.CVPipe; +import org.photonvision.vision.pipe.MutatingPipe; import org.photonvision.vision.target.TrackedTarget; public class Draw2dTargetsPipe - extends CVPipe>, Mat, Draw2dTargetsPipe.Draw2dContoursParams> { + extends MutatingPipe>, Draw2dTargetsPipe.Draw2dContoursParams> { private List m_drawnContours = new ArrayList<>(); @Override - protected Mat process(Pair> in) { + protected Void process(Pair> in) { if (!in.getRight().isEmpty() && (params.showCentroid || params.showMaximumBox @@ -115,7 +115,7 @@ public class Draw2dTargetsPipe } } - return in.getLeft(); + return null; } public static class Draw2dContoursParams { diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java index 551bd93c2..b146a6644 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java @@ -28,15 +28,15 @@ import org.opencv.core.MatOfPoint2f; import org.opencv.imgproc.Imgproc; import org.photonvision.common.util.ColorHelper; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; -import org.photonvision.vision.pipe.CVPipe; +import org.photonvision.vision.pipe.MutatingPipe; import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; public class Draw3dTargetsPipe - extends CVPipe>, Mat, Draw3dTargetsPipe.Draw3dContoursParams> { + extends MutatingPipe>, Draw3dTargetsPipe.Draw3dContoursParams> { @Override - protected Mat process(Pair> in) { + protected Void process(Pair> in) { for (var target : in.getRight()) { // draw convex hull @@ -121,7 +121,7 @@ public class Draw3dTargetsPipe } } - return in.getLeft(); + return null; } public static class Draw3dContoursParams { diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/DrawCornerDetectionPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/DrawCornerDetectionPipe.java index 47e08d899..90163a8e2 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/DrawCornerDetectionPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/DrawCornerDetectionPipe.java @@ -22,14 +22,14 @@ import org.apache.commons.lang3.tuple.Pair; import org.opencv.core.Mat; import org.opencv.core.Scalar; import org.opencv.imgproc.Imgproc; -import org.photonvision.vision.pipe.CVPipe; +import org.photonvision.vision.pipe.MutatingPipe; import org.photonvision.vision.target.TrackedTarget; public class DrawCornerDetectionPipe - extends CVPipe>, Mat, DrawCornerDetectionPipe.DrawCornerParams> { + extends MutatingPipe>, DrawCornerDetectionPipe.DrawCornerParams> { @Override - protected Mat process(Pair> in) { + protected Void process(Pair> in) { Mat image = in.getLeft(); for (var target : in.getRight()) { @@ -39,7 +39,7 @@ public class DrawCornerDetectionPipe } } - return image; + return null; } public static class DrawCornerParams { diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/ErodeDilatePipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/ErodeDilatePipe.java index b11ba1724..87850f293 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/ErodeDilatePipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/ErodeDilatePipe.java @@ -20,18 +20,18 @@ package org.photonvision.vision.pipe.impl; import org.opencv.core.Mat; import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; -import org.photonvision.vision.pipe.CVPipe; +import org.photonvision.vision.pipe.MutatingPipe; -public class ErodeDilatePipe extends CVPipe { +public class ErodeDilatePipe extends MutatingPipe { @Override - protected Mat process(Mat in) { + protected Void process(Mat in) { if (params.shouldErode()) { Imgproc.erode(in, in, params.getKernel()); } if (params.shouldDilate()) { Imgproc.dilate(in, in, params.getKernel()); } - return in; + return null; } public static class ErodeDilateParams { 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 0bb14557c..ab829f2a1 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,15 +19,15 @@ package org.photonvision.vision.pipe.impl; import org.opencv.core.Mat; import org.opencv.imgproc.Imgproc; -import org.photonvision.vision.pipe.CVPipe; +import org.photonvision.vision.pipe.MutatingPipe; -public class OutputMatPipe extends CVPipe { +public class OutputMatPipe extends MutatingPipe { @Override - protected Mat process(Mat in) { + protected Void process(Mat in) { // convert input mat Imgproc.cvtColor(in, in, Imgproc.COLOR_GRAY2BGR, 3); - return in; + return null; } public static class OutputMatParams {} diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/ResizeImagePipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/ResizeImagePipe.java index b0a7af8c6..c7a83c3bd 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/ResizeImagePipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/ResizeImagePipe.java @@ -21,10 +21,10 @@ import org.opencv.core.Mat; import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; import org.photonvision.vision.frame.FrameDivisor; -import org.photonvision.vision.pipe.CVPipe; +import org.photonvision.vision.pipe.MutatingPipe; /** Pipe that resizes an image to a given resolution */ -public class ResizeImagePipe extends CVPipe { +public class ResizeImagePipe extends MutatingPipe { public ResizeImagePipe() { setParams(ResizeImageParams.DEFAULT); @@ -34,10 +34,9 @@ public class ResizeImagePipe extends CVPipe { +public class RotateImagePipe extends MutatingPipe { public RotateImagePipe() { setParams(RotateImageParams.DEFAULT); @@ -40,9 +40,9 @@ public class RotateImagePipe extends CVPipe { @@ -56,7 +56,6 @@ public class ColoredShapePipeline private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe(); private final Mat rawInputMat = new Mat(); - private final DualMat outputMats = new DualMat(); private final Point[] rectPoints = new Point[4]; public ColoredShapePipeline() { @@ -180,123 +179,110 @@ public class ColoredShapePipeline long sumPipeNanosElapsed = 0L; - CVPipeResult rotateImageResult = rotateImagePipe.apply(frame.image.getMat()); + var rotateImageResult = rotateImagePipe.run(frame.image.getMat()); sumPipeNanosElapsed += rotateImageResult.nanosElapsed; rawInputMat.release(); frame.image.getMat().copyTo(rawInputMat); - CVPipeResult erodeDilateResult = erodeDilatePipe.apply(rotateImageResult.result); + var erodeDilateResult = erodeDilatePipe.run(rawInputMat); sumPipeNanosElapsed += erodeDilateResult.nanosElapsed; - CVPipeResult hsvPipeResult = hsvPipe.apply(erodeDilateResult.result); + CVPipeResult hsvPipeResult = hsvPipe.run(rawInputMat); sumPipeNanosElapsed += hsvPipeResult.nanosElapsed; - // the first is the raw input mat, the second is the HSVPipe result - outputMats.first = rawInputMat; - outputMats.second = hsvPipeResult.result; - - CVPipeResult> findContoursResult = findContoursPipe.apply(hsvPipeResult.result); + CVPipeResult> findContoursResult = findContoursPipe.run(hsvPipeResult.output); sumPipeNanosElapsed += findContoursResult.nanosElapsed; CVPipeResult> speckleRejectResult = - speckleRejectPipe.apply(findContoursResult.result); + speckleRejectPipe.run(findContoursResult.output); sumPipeNanosElapsed += speckleRejectResult.nanosElapsed; List shapes; if (settings.desiredShape == ContourShape.Circle) { CVPipeResult> findCirclesResult = - findCirclesPipe.apply(Pair.of(hsvPipeResult.result, speckleRejectResult.result)); + findCirclesPipe.run(Pair.of(hsvPipeResult.output, speckleRejectResult.output)); sumPipeNanosElapsed += findCirclesResult.nanosElapsed; - shapes = findCirclesResult.result; + shapes = findCirclesResult.output; } else { CVPipeResult> findPolygonsResult = - findPolygonPipe.apply(speckleRejectResult.result); + findPolygonPipe.run(speckleRejectResult.output); sumPipeNanosElapsed += findPolygonsResult.nanosElapsed; - shapes = findPolygonsResult.result; + shapes = findPolygonsResult.output; } - CVPipeResult> filterShapeResult = filterShapesPipe.apply(shapes); + CVPipeResult> filterShapeResult = filterShapesPipe.run(shapes); sumPipeNanosElapsed += filterShapeResult.nanosElapsed; CVPipeResult> groupContoursResult = - groupContoursPipe.apply( - filterShapeResult.result.stream() + groupContoursPipe.run( + filterShapeResult.output.stream() .map(CVShape::getContour) .collect(Collectors.toList())); sumPipeNanosElapsed += groupContoursResult.nanosElapsed; CVPipeResult> sortContoursResult = - sortContoursPipe.apply(groupContoursResult.result); + sortContoursPipe.run(groupContoursResult.output); sumPipeNanosElapsed += sortContoursResult.nanosElapsed; CVPipeResult> collect2dTargetsResult = - collect2dTargetsPipe.apply(sortContoursResult.result); + collect2dTargetsPipe.run(sortContoursResult.output); sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed; List targetList; if (settings.solvePNPEnabled && settings.desiredShape == ContourShape.Circle) { - var cornerDetectionResult = cornerDetectionPipe.apply(collect2dTargetsResult.result); - collect2dTargetsResult.result.forEach( + var cornerDetectionResult = cornerDetectionPipe.run(collect2dTargetsResult.output); + collect2dTargetsResult.output.forEach( shape -> { shape.getMinAreaRect().points(rectPoints); shape.setCorners(Arrays.asList(rectPoints)); }); sumPipeNanosElapsed += cornerDetectionResult.nanosElapsed; - var solvePNPResult = solvePNPPipe.apply(cornerDetectionResult.result); + var solvePNPResult = solvePNPPipe.run(cornerDetectionResult.output); sumPipeNanosElapsed += solvePNPResult.nanosElapsed; - targetList = solvePNPResult.result; + targetList = solvePNPResult.output; } else { - targetList = collect2dTargetsResult.result; + targetList = collect2dTargetsResult.output; } - // the first is the raw input mat, the second is the HSVPipe result - CVPipeResult drawOnInputResult, drawOnOutputResult; - - CVPipeResult draw2dCrosshairResultOnInput = - draw2dCrosshairPipe.apply(Pair.of(outputMats.first, targetList)); + // Draw 2D Crosshair on input and output + var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(rawInputMat, targetList)); sumPipeNanosElapsed += draw2dCrosshairResultOnInput.nanosElapsed; - CVPipeResult draw2dCrosshairResultOnOutput = - draw2dCrosshairPipe.apply(Pair.of(outputMats.second, targetList)); + var draw2dCrosshairResultOnOutput = + draw2dCrosshairPipe.run(Pair.of(hsvPipeResult.output, targetList)); sumPipeNanosElapsed += draw2dCrosshairResultOnOutput.nanosElapsed; - CVPipeResult draw2dContoursResultOnInput = - draw2DTargetsPipe.apply( - Pair.of(draw2dCrosshairResultOnInput.result, collect2dTargetsResult.result)); + // Draw 2D contours on input and output + var draw2dContoursResultOnInput = + draw2DTargetsPipe.run(Pair.of(rawInputMat, collect2dTargetsResult.output)); sumPipeNanosElapsed += draw2dContoursResultOnInput.nanosElapsed; - CVPipeResult draw2dContoursResultOnOutput = - draw2DTargetsPipe.apply( - Pair.of(draw2dCrosshairResultOnOutput.result, collect2dTargetsResult.result)); + var draw2dContoursResultOnOutput = + draw2DTargetsPipe.run(Pair.of(hsvPipeResult.output, collect2dTargetsResult.output)); sumPipeNanosElapsed += draw2dContoursResultOnOutput.nanosElapsed; if (settings.solvePNPEnabled && settings.desiredShape == ContourShape.Circle) { - drawOnInputResult = - draw3dTargetsPipe.apply( - Pair.of(draw2dContoursResultOnInput.result, collect2dTargetsResult.result)); + var drawOnInputResult = + draw3dTargetsPipe.run(Pair.of(rawInputMat, collect2dTargetsResult.output)); sumPipeNanosElapsed += drawOnInputResult.nanosElapsed; - drawOnOutputResult = - draw3dTargetsPipe.apply( - Pair.of(draw2dContoursResultOnOutput.result, collect2dTargetsResult.result)); + var drawOnOutputResult = + draw3dTargetsPipe.run(Pair.of(hsvPipeResult.output, collect2dTargetsResult.output)); 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); + var outputMatPipeResult = outputMatPipe.run(hsvPipeResult.output); sumPipeNanosElapsed += outputMatPipeResult.nanosElapsed; return new CVPipelineResult( MathUtils.nanosToMillis(sumPipeNanosElapsed), targetList, - new Frame(new CVMat(outputMats.second), frame.frameStaticProperties), - new Frame(new CVMat(outputMats.first), frame.frameStaticProperties)); + new Frame(new CVMat(hsvPipeResult.output), frame.frameStaticProperties), + new Frame(new CVMat(rawInputMat), frame.frameStaticProperties)); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java index 789c7b7ce..88c42478f 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java @@ -53,15 +53,16 @@ public class DriverModePipeline @Override public DriverModePipelineResult process(Frame frame, DriverModePipelineSettings settings) { // apply pipes - var rotateImageResult = rotateImagePipe.apply(frame.image.getMat()); - var draw2dCrosshairResult = - draw2dCrosshairPipe.apply(Pair.of(rotateImageResult.result, List.of())); + var inputMat = frame.image.getMat(); + + var rotateImageResult = rotateImagePipe.run(inputMat); + var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of())); // calculate elapsed nanoseconds long totalNanos = rotateImageResult.nanosElapsed + draw2dCrosshairResult.nanosElapsed; return new DriverModePipelineResult( MathUtils.nanosToMillis(totalNanos), - new Frame(new CVMat(draw2dCrosshairResult.result), frame.frameStaticProperties)); + new Frame(new CVMat(inputMat), 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 384d461b3..2b1b2df43 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 @@ -25,8 +25,7 @@ import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.opencv.CVMat; import org.photonvision.vision.opencv.Contour; -import org.photonvision.vision.opencv.DualMat; -import org.photonvision.vision.pipe.CVPipeResult; +import org.photonvision.vision.pipe.CVPipe.CVPipeResult; import org.photonvision.vision.pipe.impl.Collect2dTargetsPipe; import org.photonvision.vision.pipe.impl.CornerDetectionPipe; import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe; @@ -47,7 +46,7 @@ import org.photonvision.vision.target.PotentialTarget; import org.photonvision.vision.target.TrackedTarget; /** Represents a pipeline for tracking retro-reflective targets. */ -@SuppressWarnings({"UnusedAssignment", "DuplicatedCode"}) +@SuppressWarnings({"DuplicatedCode"}) public class ReflectivePipeline extends CVPipeline { private final RotateImagePipe rotateImagePipe = new RotateImagePipe(); @@ -67,7 +66,6 @@ public class ReflectivePipeline extends CVPipeline rotateImageResult = rotateImagePipe.apply(frame.image.getMat()); + var rotateImageResult = rotateImagePipe.run(frame.image.getMat()); sumPipeNanosElapsed += rotateImageResult.nanosElapsed; rawInputMat.release(); frame.image.getMat().copyTo(rawInputMat); - CVPipeResult erodeDilateResult = erodeDilatePipe.apply(rotateImageResult.result); + var erodeDilateResult = erodeDilatePipe.run(rawInputMat); sumPipeNanosElapsed += erodeDilateResult.nanosElapsed; - CVPipeResult hsvPipeResult = hsvPipe.apply(erodeDilateResult.result); + CVPipeResult hsvPipeResult = hsvPipe.run(rawInputMat); sumPipeNanosElapsed += hsvPipeResult.nanosElapsed; - // the first is the raw input mat, the second is the HSVPipe result - outputMats.first = rawInputMat; - outputMats.second = hsvPipeResult.result; - - CVPipeResult> findContoursResult = findContoursPipe.apply(hsvPipeResult.result); + CVPipeResult> findContoursResult = findContoursPipe.run(hsvPipeResult.output); sumPipeNanosElapsed += findContoursResult.nanosElapsed; CVPipeResult> speckleRejectResult = - speckleRejectPipe.apply(findContoursResult.result); + speckleRejectPipe.run(findContoursResult.output); sumPipeNanosElapsed += speckleRejectResult.nanosElapsed; CVPipeResult> filterContoursResult = - filterContoursPipe.apply(speckleRejectResult.result); + filterContoursPipe.run(speckleRejectResult.output); sumPipeNanosElapsed += filterContoursResult.nanosElapsed; CVPipeResult> groupContoursResult = - groupContoursPipe.apply(filterContoursResult.result); + groupContoursPipe.run(filterContoursResult.output); sumPipeNanosElapsed += groupContoursResult.nanosElapsed; CVPipeResult> sortContoursResult = - sortContoursPipe.apply(groupContoursResult.result); + sortContoursPipe.run(groupContoursResult.output); sumPipeNanosElapsed += sortContoursResult.nanosElapsed; CVPipeResult> collect2dTargetsResult = - collect2dTargetsPipe.apply(sortContoursResult.result); + collect2dTargetsPipe.run(sortContoursResult.output); sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed; List targetList; // 3d stuff if (settings.solvePNPEnabled) { - var cornerDetectionResult = cornerDetectionPipe.apply(collect2dTargetsResult.result); + var cornerDetectionResult = cornerDetectionPipe.run(collect2dTargetsResult.output); sumPipeNanosElapsed += cornerDetectionResult.nanosElapsed; - var solvePNPResult = solvePNPPipe.apply(cornerDetectionResult.result); + var solvePNPResult = solvePNPPipe.run(cornerDetectionResult.output); sumPipeNanosElapsed += solvePNPResult.nanosElapsed; - targetList = solvePNPResult.result; + targetList = solvePNPResult.output; } else { - targetList = collect2dTargetsResult.result; + targetList = collect2dTargetsResult.output; } - // the first is the raw input mat, the second is the HSVPipe result - CVPipeResult drawOnInputResult, drawOnOutputResult; - // Draw 2D Crosshair on input and output - CVPipeResult draw2dCrosshairResultOnInput = - draw2dCrosshairPipe.apply(Pair.of(outputMats.first, targetList)); + var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(rawInputMat, targetList)); sumPipeNanosElapsed += draw2dCrosshairResultOnInput.nanosElapsed; - CVPipeResult draw2dCrosshairResultOnOutput = - draw2dCrosshairPipe.apply(Pair.of(outputMats.second, targetList)); + var draw2dCrosshairResultOnOutput = + draw2dCrosshairPipe.run(Pair.of(hsvPipeResult.output, targetList)); sumPipeNanosElapsed += draw2dCrosshairResultOnOutput.nanosElapsed; // Draw 2D contours on input and output - CVPipeResult draw2dContoursResultOnInput = - draw2DTargetsPipe.apply( - Pair.of(draw2dCrosshairResultOnInput.result, collect2dTargetsResult.result)); - sumPipeNanosElapsed += draw2dCrosshairResultOnInput.nanosElapsed; + var draw2dContoursResultOnInput = + draw2DTargetsPipe.run(Pair.of(rawInputMat, collect2dTargetsResult.output)); + sumPipeNanosElapsed += draw2dContoursResultOnInput.nanosElapsed; - CVPipeResult draw2dContoursResultOnOutput = - draw2DTargetsPipe.apply( - Pair.of(draw2dCrosshairResultOnOutput.result, collect2dTargetsResult.result)); + var draw2dContoursResultOnOutput = + draw2DTargetsPipe.run(Pair.of(hsvPipeResult.output, collect2dTargetsResult.output)); sumPipeNanosElapsed += draw2dContoursResultOnOutput.nanosElapsed; // Draw 3D Targets on input and output if necessary if (settings.solvePNPEnabled) { - drawOnInputResult = - draw3dTargetsPipe.apply( - Pair.of(draw2dContoursResultOnInput.result, collect2dTargetsResult.result)); + var drawOnInputResult = + draw3dTargetsPipe.run(Pair.of(rawInputMat, collect2dTargetsResult.output)); sumPipeNanosElapsed += drawOnInputResult.nanosElapsed; - drawOnOutputResult = - draw3dTargetsPipe.apply( - Pair.of(draw2dContoursResultOnOutput.result, collect2dTargetsResult.result)); + var drawOnOutputResult = + draw3dTargetsPipe.run(Pair.of(hsvPipeResult.output, collect2dTargetsResult.output)); 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); + var outputMatPipeResult = outputMatPipe.run(hsvPipeResult.output); sumPipeNanosElapsed += outputMatPipeResult.nanosElapsed; return new CVPipelineResult( MathUtils.nanosToMillis(sumPipeNanosElapsed), targetList, - new Frame(new CVMat(outputMats.second), frame.frameStaticProperties), - new Frame(new CVMat(outputMats.first), frame.frameStaticProperties)); + new Frame(new CVMat(hsvPipeResult.output), frame.frameStaticProperties), + new Frame(new CVMat(rawInputMat), frame.frameStaticProperties)); } } diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java index 159f2d1bb..c43a93a4f 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java @@ -30,11 +30,9 @@ import org.opencv.core.Size; import org.opencv.highgui.HighGui; import org.opencv.imgcodecs.Imgcodecs; import org.photonvision.common.util.TestUtils; -import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.opencv.CVMat; -import org.photonvision.vision.pipe.CVPipeResult; import org.photonvision.vision.pipe.impl.Calibrate3dPipe; import org.photonvision.vision.pipe.impl.FindBoardCornersPipe; @@ -57,16 +55,15 @@ public class Calibrate3dPipeTest { FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe(); findBoardCornersPipe.setParams( new FindBoardCornersPipe.FindCornersPipeParams(11, 4, false, 15)); - CVPipeResult>> findBoardCornersPipeOutput = findBoardCornersPipe.apply(frames); + var findBoardCornersPipeOutput = findBoardCornersPipe.run(frames); Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe(); calibrate3dPipe.setParams(new Calibrate3dPipe.CalibratePipeParams(new Size(640, 480))); - CVPipeResult calibrate3dPipeOutput = - calibrate3dPipe.apply(findBoardCornersPipeOutput.result); - assertTrue(calibrate3dPipeOutput.result.perViewErrors.length > 0); + var calibrate3dPipeOutput = calibrate3dPipe.run(findBoardCornersPipeOutput.output); + assertTrue(calibrate3dPipeOutput.output.perViewErrors.length > 0); System.out.println( - "Per View Errors: " + Arrays.toString(calibrate3dPipeOutput.result.perViewErrors)); + "Per View Errors: " + Arrays.toString(calibrate3dPipeOutput.output.perViewErrors)); } @Test