mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Stream threading (#110)
* Condense pipeline settings classes * Add OutputStreamPipeline
This commit is contained in:
Banks T
@@ -17,6 +17,7 @@
|
||||
|
||||
package org.photonvision.vision.frame;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import org.opencv.core.Mat;
|
||||
import org.photonvision.vision.opencv.CVMat;
|
||||
import org.photonvision.vision.opencv.Releasable;
|
||||
@@ -36,6 +37,12 @@ public class Frame implements Releasable {
|
||||
this(image, System.nanoTime(), frameStaticProperties);
|
||||
}
|
||||
|
||||
public Frame() {
|
||||
timestampNanos = 0;
|
||||
image = new CVMat(new Mat());
|
||||
frameStaticProperties = new FrameStaticProperties(0, 0, 0, new Rotation2d(), null);
|
||||
}
|
||||
|
||||
public void copyTo(Frame destFrame) {
|
||||
image.getMat().copyTo(destFrame.image.getMat());
|
||||
}
|
||||
|
||||
@@ -21,8 +21,12 @@ import java.util.Objects;
|
||||
import org.opencv.core.Point;
|
||||
import org.photonvision.common.util.numbers.DoubleCouple;
|
||||
import org.photonvision.common.util.numbers.IntegerCouple;
|
||||
import org.photonvision.vision.opencv.ContourGroupingMode;
|
||||
import org.photonvision.vision.opencv.ContourIntersectionDirection;
|
||||
import org.photonvision.vision.opencv.ContourSortMode;
|
||||
import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
|
||||
import org.photonvision.vision.target.RobotOffsetPointMode;
|
||||
import org.photonvision.vision.target.TargetModel;
|
||||
import org.photonvision.vision.target.TargetOffsetPointEdge;
|
||||
import org.photonvision.vision.target.TargetOrientation;
|
||||
|
||||
@@ -71,10 +75,28 @@ public class AdvancedPipelineSettings extends CVPipelineSettings {
|
||||
public Point offsetDualPointB = new Point();
|
||||
public double offsetDualPointBArea = 0;
|
||||
|
||||
// how many contours to attempt to group (Single, Dual)
|
||||
public ContourGroupingMode contourGroupingMode = ContourGroupingMode.Single;
|
||||
|
||||
// the direction in which contours must intersect to be considered intersecting
|
||||
public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;
|
||||
|
||||
// 3d settings
|
||||
public boolean solvePNPEnabled = false;
|
||||
public TargetModel targetModel = TargetModel.get2020Target();
|
||||
|
||||
// Corner detection settings
|
||||
public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
|
||||
CornerDetectionPipe.DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS;
|
||||
public boolean cornerDetectionUseConvexHulls = true;
|
||||
public boolean cornerDetectionExactSideCount = false;
|
||||
public int cornerDetectionSideCount = 4;
|
||||
public double cornerDetectionAccuracyPercentage = 10;
|
||||
|
||||
@Override
|
||||
public boolean equals(Object o) {
|
||||
if (this == o) return true;
|
||||
if (o == null || getClass() != o.getClass()) return false;
|
||||
if (!(o instanceof AdvancedPipelineSettings)) return false;
|
||||
if (!super.equals(o)) return false;
|
||||
AdvancedPipelineSettings that = (AdvancedPipelineSettings) o;
|
||||
return outputShouldDraw == that.outputShouldDraw
|
||||
@@ -82,23 +104,31 @@ public class AdvancedPipelineSettings extends CVPipelineSettings {
|
||||
&& erode == that.erode
|
||||
&& dilate == that.dilate
|
||||
&& contourSpecklePercentage == that.contourSpecklePercentage
|
||||
&& Double.compare(that.offsetDualPointA.x, offsetDualPointA.x) == 0
|
||||
&& Double.compare(that.offsetDualPointA.y, offsetDualPointA.y) == 0
|
||||
&& Double.compare(that.offsetDualPointAArea, offsetDualPointAArea) == 0
|
||||
&& Double.compare(that.offsetDualPointB.x, offsetDualPointB.x) == 0
|
||||
&& Double.compare(that.offsetDualPointB.y, offsetDualPointB.y) == 0
|
||||
&& Double.compare(that.offsetDualPointBArea, offsetDualPointBArea) == 0
|
||||
&& hsvHue.equals(that.hsvHue)
|
||||
&& hsvSaturation.equals(that.hsvSaturation)
|
||||
&& hsvValue.equals(that.hsvValue)
|
||||
&& contourArea.equals(that.contourArea)
|
||||
&& contourRatio.equals(that.contourRatio)
|
||||
&& contourFullness.equals(that.contourFullness)
|
||||
&& solvePNPEnabled == that.solvePNPEnabled
|
||||
&& cornerDetectionUseConvexHulls == that.cornerDetectionUseConvexHulls
|
||||
&& cornerDetectionExactSideCount == that.cornerDetectionExactSideCount
|
||||
&& cornerDetectionSideCount == that.cornerDetectionSideCount
|
||||
&& Double.compare(that.cornerDetectionAccuracyPercentage, cornerDetectionAccuracyPercentage)
|
||||
== 0
|
||||
&& Objects.equals(hsvHue, that.hsvHue)
|
||||
&& Objects.equals(hsvSaturation, that.hsvSaturation)
|
||||
&& Objects.equals(hsvValue, that.hsvValue)
|
||||
&& Objects.equals(contourArea, that.contourArea)
|
||||
&& Objects.equals(contourRatio, that.contourRatio)
|
||||
&& Objects.equals(contourFullness, that.contourFullness)
|
||||
&& contourSortMode == that.contourSortMode
|
||||
&& contourTargetOffsetPointEdge == that.contourTargetOffsetPointEdge
|
||||
&& contourTargetOrientation == that.contourTargetOrientation
|
||||
&& offsetRobotOffsetMode == that.offsetRobotOffsetMode
|
||||
&& offsetSinglePoint.equals(that.offsetSinglePoint);
|
||||
&& Objects.equals(offsetSinglePoint, that.offsetSinglePoint)
|
||||
&& Objects.equals(offsetDualPointA, that.offsetDualPointA)
|
||||
&& Objects.equals(offsetDualPointB, that.offsetDualPointB)
|
||||
&& contourGroupingMode == that.contourGroupingMode
|
||||
&& contourIntersection == that.contourIntersection
|
||||
&& Objects.equals(targetModel, that.targetModel)
|
||||
&& cornerDetectionStrategy == that.cornerDetectionStrategy;
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -124,6 +154,15 @@ public class AdvancedPipelineSettings extends CVPipelineSettings {
|
||||
offsetDualPointA,
|
||||
offsetDualPointAArea,
|
||||
offsetDualPointB,
|
||||
offsetDualPointBArea);
|
||||
offsetDualPointBArea,
|
||||
contourGroupingMode,
|
||||
contourIntersection,
|
||||
solvePNPEnabled,
|
||||
targetModel,
|
||||
cornerDetectionStrategy,
|
||||
cornerDetectionUseConvexHulls,
|
||||
cornerDetectionExactSideCount,
|
||||
cornerDetectionSideCount,
|
||||
cornerDetectionAccuracyPercentage);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -158,7 +158,9 @@ public class ColoredShapePipeline
|
||||
|
||||
var solvePNPParams =
|
||||
new SolvePNPPipe.SolvePNPPipeParams(
|
||||
settings.cameraCalibration, settings.cameraPitch, settings.targetModel);
|
||||
frameStaticProperties.cameraCalibration,
|
||||
frameStaticProperties.cameraPitch,
|
||||
settings.targetModel);
|
||||
solvePNPPipe.setParams(solvePNPParams);
|
||||
|
||||
Draw2dTargetsPipe.Draw2dTargetsParams draw2DTargetsParams =
|
||||
@@ -178,10 +180,12 @@ public class ColoredShapePipeline
|
||||
frameStaticProperties);
|
||||
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
|
||||
|
||||
var draw3dContoursParams =
|
||||
var draw3dTargetsParams =
|
||||
new Draw3dTargetsPipe.Draw3dContoursParams(
|
||||
settings.outputShouldDraw, settings.cameraCalibration, settings.targetModel);
|
||||
draw3dTargetsPipe.setParams(draw3dContoursParams);
|
||||
settings.outputShouldDraw,
|
||||
frameStaticProperties.cameraCalibration,
|
||||
settings.targetModel);
|
||||
draw3dTargetsPipe.setParams(draw3dTargetsParams);
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
@@ -18,14 +18,8 @@
|
||||
package org.photonvision.vision.pipeline;
|
||||
|
||||
import com.fasterxml.jackson.annotation.JsonTypeName;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import java.util.Objects;
|
||||
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
|
||||
import org.photonvision.vision.opencv.ContourGroupingMode;
|
||||
import org.photonvision.vision.opencv.ContourIntersectionDirection;
|
||||
import org.photonvision.vision.opencv.ContourShape;
|
||||
import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
|
||||
import org.photonvision.vision.target.TargetModel;
|
||||
|
||||
@JsonTypeName("ColoredShapePipelineSettings")
|
||||
public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
|
||||
@@ -42,25 +36,6 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
|
||||
public int minDist = 10;
|
||||
public int maxCannyThresh = 90;
|
||||
public int accuracy = 20;
|
||||
// how many contours to attempt to group (Single, Dual)
|
||||
public ContourGroupingMode contourGroupingMode = ContourGroupingMode.Single;
|
||||
|
||||
// the direction in which contours must intersect to be considered intersecting
|
||||
public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;
|
||||
|
||||
// 3d settings
|
||||
public boolean solvePNPEnabled = false;
|
||||
public CameraCalibrationCoefficients cameraCalibration;
|
||||
public TargetModel targetModel;
|
||||
public Rotation2d cameraPitch = Rotation2d.fromDegrees(0.0); // TODO where should pitch live?
|
||||
|
||||
// Corner detection settings
|
||||
public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
|
||||
CornerDetectionPipe.DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS;
|
||||
public boolean cornerDetectionUseConvexHulls = true;
|
||||
public boolean cornerDetectionExactSideCount = false;
|
||||
public int cornerDetectionSideCount = 4;
|
||||
public double cornerDetectionAccuracyPercentage = 10;
|
||||
|
||||
public ColoredShapePipelineSettings() {
|
||||
super();
|
||||
@@ -70,7 +45,7 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
|
||||
@Override
|
||||
public boolean equals(Object o) {
|
||||
if (this == o) return true;
|
||||
if (o == null || getClass() != o.getClass()) return false;
|
||||
if (!(o instanceof ColoredShapePipelineSettings)) return false;
|
||||
if (!super.equals(o)) return false;
|
||||
ColoredShapePipelineSettings that = (ColoredShapePipelineSettings) o;
|
||||
return Double.compare(that.minArea, minArea) == 0
|
||||
@@ -84,19 +59,7 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
|
||||
&& minDist == that.minDist
|
||||
&& maxCannyThresh == that.maxCannyThresh
|
||||
&& accuracy == that.accuracy
|
||||
&& solvePNPEnabled == that.solvePNPEnabled
|
||||
&& cornerDetectionUseConvexHulls == that.cornerDetectionUseConvexHulls
|
||||
&& cornerDetectionExactSideCount == that.cornerDetectionExactSideCount
|
||||
&& cornerDetectionSideCount == that.cornerDetectionSideCount
|
||||
&& Double.compare(that.cornerDetectionAccuracyPercentage, cornerDetectionAccuracyPercentage)
|
||||
== 0
|
||||
&& desiredShape == that.desiredShape
|
||||
&& contourGroupingMode == that.contourGroupingMode
|
||||
&& contourIntersection == that.contourIntersection
|
||||
&& Objects.equals(cameraCalibration, that.cameraCalibration)
|
||||
&& Objects.equals(targetModel, that.targetModel)
|
||||
&& Objects.equals(cameraPitch, that.cameraPitch)
|
||||
&& cornerDetectionStrategy == that.cornerDetectionStrategy;
|
||||
&& desiredShape == that.desiredShape;
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -114,17 +77,6 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
|
||||
maxRadius,
|
||||
minDist,
|
||||
maxCannyThresh,
|
||||
accuracy,
|
||||
contourGroupingMode,
|
||||
contourIntersection,
|
||||
solvePNPEnabled,
|
||||
cameraCalibration,
|
||||
targetModel,
|
||||
cameraPitch,
|
||||
cornerDetectionStrategy,
|
||||
cornerDetectionUseConvexHulls,
|
||||
cornerDetectionExactSideCount,
|
||||
cornerDetectionSideCount,
|
||||
cornerDetectionAccuracyPercentage);
|
||||
accuracy);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,130 @@
|
||||
/*
|
||||
* Copyright (C) 2020 Photon Vision.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
package org.photonvision.vision.pipeline;
|
||||
|
||||
import java.util.List;
|
||||
import org.apache.commons.lang3.tuple.Pair;
|
||||
import org.apache.commons.lang3.tuple.Triple;
|
||||
import org.photonvision.common.util.math.MathUtils;
|
||||
import org.photonvision.vision.frame.Frame;
|
||||
import org.photonvision.vision.frame.FrameStaticProperties;
|
||||
import org.photonvision.vision.opencv.CVMat;
|
||||
import org.photonvision.vision.opencv.DualOffsetValues;
|
||||
import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe;
|
||||
import org.photonvision.vision.pipe.impl.Draw2dTargetsPipe;
|
||||
import org.photonvision.vision.pipe.impl.Draw3dTargetsPipe;
|
||||
import org.photonvision.vision.pipe.impl.OutputMatPipe;
|
||||
import org.photonvision.vision.pipeline.result.CVPipelineResult;
|
||||
import org.photonvision.vision.target.TrackedTarget;
|
||||
|
||||
/**
|
||||
* This is a "fake" pipeline that is just used to move identical pipe sets out of real pipelines. It
|
||||
* shall not get its settings saved, nor shall it be managed by PipelineManager
|
||||
*/
|
||||
public class OutputStreamPipeline {
|
||||
|
||||
private final OutputMatPipe outputMatPipe = new OutputMatPipe();
|
||||
private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
|
||||
private final Draw2dTargetsPipe draw2dTargetsPipe = new Draw2dTargetsPipe();
|
||||
private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
|
||||
|
||||
private final long[] pipeProfileNanos = new long[10];
|
||||
|
||||
protected void setPipeParams(
|
||||
FrameStaticProperties frameStaticProperties, AdvancedPipelineSettings settings) {
|
||||
|
||||
var dualOffsetValues =
|
||||
new DualOffsetValues(
|
||||
settings.offsetDualPointA,
|
||||
settings.offsetDualPointAArea,
|
||||
settings.offsetDualPointB,
|
||||
settings.offsetDualPointBArea);
|
||||
|
||||
var draw2DTargetsParams =
|
||||
new Draw2dTargetsPipe.Draw2dTargetsParams(
|
||||
settings.outputShouldDraw, settings.outputShowMultipleTargets);
|
||||
draw2dTargetsPipe.setParams(draw2DTargetsParams);
|
||||
|
||||
var draw2dCrosshairParams =
|
||||
new Draw2dCrosshairPipe.Draw2dCrosshairParams(
|
||||
settings.outputShouldDraw,
|
||||
settings.offsetRobotOffsetMode,
|
||||
settings.offsetSinglePoint,
|
||||
dualOffsetValues,
|
||||
frameStaticProperties);
|
||||
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
|
||||
|
||||
var draw3dTargetsParams =
|
||||
new Draw3dTargetsPipe.Draw3dContoursParams(
|
||||
settings.outputShouldDraw,
|
||||
frameStaticProperties.cameraCalibration,
|
||||
settings.targetModel);
|
||||
draw3dTargetsPipe.setParams(draw3dTargetsParams);
|
||||
}
|
||||
|
||||
public CVPipelineResult process(
|
||||
Frame inputFrame,
|
||||
Frame outputFrame,
|
||||
AdvancedPipelineSettings settings,
|
||||
List<TrackedTarget> targetsToDraw,
|
||||
double fpsToDraw) {
|
||||
setPipeParams(inputFrame.frameStaticProperties, settings);
|
||||
var inMat = inputFrame.image.getMat();
|
||||
var outMat = outputFrame.image.getMat();
|
||||
|
||||
long sumPipeNanosElapsed = 0L;
|
||||
|
||||
// Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
|
||||
var outputMatPipeResult = outputMatPipe.run(outMat);
|
||||
sumPipeNanosElapsed += pipeProfileNanos[0] = outputMatPipeResult.nanosElapsed;
|
||||
|
||||
// Draw 2D Crosshair on input and output
|
||||
var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[1] = draw2dCrosshairResultOnInput.nanosElapsed;
|
||||
|
||||
var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[2] = draw2dCrosshairResultOnOutput.nanosElapsed;
|
||||
|
||||
// Draw 2D contours on input and output
|
||||
var draw2dTargetsOnInput =
|
||||
draw2dTargetsPipe.run(Triple.of(inMat, targetsToDraw, (int) fpsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dTargetsOnInput.nanosElapsed;
|
||||
|
||||
var draw2dTargetsOnOutput =
|
||||
draw2dTargetsPipe.run(Triple.of(outMat, targetsToDraw, (int) fpsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dTargetsOnOutput.nanosElapsed;
|
||||
|
||||
// Draw 3D Targets on input and output if necessary
|
||||
if (settings.solvePNPEnabled) {
|
||||
var drawOnInputResult = draw3dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[5] = drawOnInputResult.nanosElapsed;
|
||||
|
||||
var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[6] = drawOnOutputResult.nanosElapsed;
|
||||
} else {
|
||||
pipeProfileNanos[5] = 0;
|
||||
pipeProfileNanos[6] = 0;
|
||||
}
|
||||
|
||||
return new CVPipelineResult(
|
||||
MathUtils.nanosToMillis(sumPipeNanosElapsed),
|
||||
targetsToDraw,
|
||||
new Frame(new CVMat(outMat), outputFrame.frameStaticProperties),
|
||||
new Frame(new CVMat(inMat), inputFrame.frameStaticProperties));
|
||||
}
|
||||
}
|
||||
@@ -18,8 +18,6 @@
|
||||
package org.photonvision.vision.pipeline;
|
||||
|
||||
import java.util.List;
|
||||
import org.apache.commons.lang3.tuple.Pair;
|
||||
import org.apache.commons.lang3.tuple.Triple;
|
||||
import org.opencv.core.Mat;
|
||||
import org.photonvision.common.util.math.MathUtils;
|
||||
import org.photonvision.vision.frame.Frame;
|
||||
@@ -48,11 +46,11 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
|
||||
private final Collect2dTargetsPipe collect2dTargetsPipe = new Collect2dTargetsPipe();
|
||||
private final CornerDetectionPipe cornerDetectionPipe = new CornerDetectionPipe();
|
||||
private final SolvePNPPipe solvePNPPipe = new SolvePNPPipe();
|
||||
private final OutputMatPipe outputMatPipe = new OutputMatPipe();
|
||||
private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
|
||||
private final Draw2dTargetsPipe draw2dTargetsPipe = new Draw2dTargetsPipe();
|
||||
private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
|
||||
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
|
||||
// private final OutputMatPipe outputMatPipe = new OutputMatPipe();
|
||||
// private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
|
||||
// private final Draw2dTargetsPipe draw2dTargetsPipe = new Draw2dTargetsPipe();
|
||||
// private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
|
||||
// private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
|
||||
|
||||
private final Mat rawInputMat = new Mat();
|
||||
private final long[] pipeProfileNanos = new long[PipelineProfiler.ReflectivePipeCount];
|
||||
@@ -69,35 +67,33 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
|
||||
protected void setPipeParams(
|
||||
FrameStaticProperties frameStaticProperties, ReflectivePipelineSettings settings) {
|
||||
|
||||
DualOffsetValues dualOffsetValues =
|
||||
var dualOffsetValues =
|
||||
new DualOffsetValues(
|
||||
settings.offsetDualPointA,
|
||||
settings.offsetDualPointAArea,
|
||||
settings.offsetDualPointB,
|
||||
settings.offsetDualPointBArea);
|
||||
|
||||
RotateImagePipe.RotateImageParams rotateImageParams =
|
||||
new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
|
||||
var rotateImageParams = new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
|
||||
rotateImagePipe.setParams(rotateImageParams);
|
||||
|
||||
ErodeDilatePipe.ErodeDilateParams erodeDilateParams =
|
||||
var erodeDilateParams =
|
||||
new ErodeDilatePipe.ErodeDilateParams(settings.erode, settings.dilate, 5);
|
||||
// TODO: add kernel size to pipeline settings
|
||||
erodeDilatePipe.setParams(erodeDilateParams);
|
||||
|
||||
HSVPipe.HSVParams hsvParams =
|
||||
var hsvParams =
|
||||
new HSVPipe.HSVParams(settings.hsvHue, settings.hsvSaturation, settings.hsvValue);
|
||||
hsvPipe.setParams(hsvParams);
|
||||
|
||||
FindContoursPipe.FindContoursParams findContoursParams =
|
||||
new FindContoursPipe.FindContoursParams();
|
||||
var findContoursParams = new FindContoursPipe.FindContoursParams();
|
||||
findContoursPipe.setParams(findContoursParams);
|
||||
|
||||
SpeckleRejectPipe.SpeckleRejectParams speckleRejectParams =
|
||||
var speckleRejectParams =
|
||||
new SpeckleRejectPipe.SpeckleRejectParams(settings.contourSpecklePercentage);
|
||||
speckleRejectPipe.setParams(speckleRejectParams);
|
||||
|
||||
FilterContoursPipe.FilterContoursParams filterContoursParams =
|
||||
var filterContoursParams =
|
||||
new FilterContoursPipe.FilterContoursParams(
|
||||
settings.contourArea,
|
||||
settings.contourRatio,
|
||||
@@ -105,19 +101,19 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
|
||||
frameStaticProperties);
|
||||
filterContoursPipe.setParams(filterContoursParams);
|
||||
|
||||
GroupContoursPipe.GroupContoursParams groupContoursParams =
|
||||
var groupContoursParams =
|
||||
new GroupContoursPipe.GroupContoursParams(
|
||||
settings.contourGroupingMode, settings.contourIntersection);
|
||||
groupContoursPipe.setParams(groupContoursParams);
|
||||
|
||||
SortContoursPipe.SortContoursParams sortContoursParams =
|
||||
var sortContoursParams =
|
||||
new SortContoursPipe.SortContoursParams(
|
||||
settings.contourSortMode,
|
||||
settings.outputShowMultipleTargets ? 5 : 1, // TODO don't hardcode?
|
||||
frameStaticProperties);
|
||||
sortContoursPipe.setParams(sortContoursParams);
|
||||
|
||||
Collect2dTargetsPipe.Collect2dTargetsParams collect2dTargetsParams =
|
||||
var collect2dTargetsParams =
|
||||
new Collect2dTargetsPipe.Collect2dTargetsParams(
|
||||
settings.offsetRobotOffsetMode,
|
||||
settings.offsetSinglePoint,
|
||||
@@ -127,35 +123,35 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
|
||||
frameStaticProperties);
|
||||
collect2dTargetsPipe.setParams(collect2dTargetsParams);
|
||||
|
||||
var params =
|
||||
var cornerDetectionPipeParams =
|
||||
new CornerDetectionPipe.CornerDetectionPipeParameters(
|
||||
settings.cornerDetectionStrategy,
|
||||
settings.cornerDetectionUseConvexHulls,
|
||||
settings.cornerDetectionExactSideCount,
|
||||
settings.cornerDetectionSideCount,
|
||||
settings.cornerDetectionAccuracyPercentage);
|
||||
cornerDetectionPipe.setParams(params);
|
||||
cornerDetectionPipe.setParams(cornerDetectionPipeParams);
|
||||
|
||||
Draw2dTargetsPipe.Draw2dTargetsParams draw2DTargetsParams =
|
||||
new Draw2dTargetsPipe.Draw2dTargetsParams(
|
||||
settings.outputShouldDraw, settings.outputShowMultipleTargets);
|
||||
draw2dTargetsPipe.setParams(draw2DTargetsParams);
|
||||
|
||||
Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
|
||||
new Draw2dCrosshairPipe.Draw2dCrosshairParams(
|
||||
settings.outputShouldDraw,
|
||||
settings.offsetRobotOffsetMode,
|
||||
settings.offsetSinglePoint,
|
||||
dualOffsetValues,
|
||||
frameStaticProperties);
|
||||
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
|
||||
|
||||
var draw3dContoursParams =
|
||||
new Draw3dTargetsPipe.Draw3dContoursParams(
|
||||
settings.outputShouldDraw,
|
||||
frameStaticProperties.cameraCalibration,
|
||||
settings.targetModel);
|
||||
draw3dTargetsPipe.setParams(draw3dContoursParams);
|
||||
// var draw2DTargetsParams =
|
||||
// new Draw2dTargetsPipe.Draw2dTargetsParams(
|
||||
// settings.outputShouldDraw, settings.outputShowMultipleTargets);
|
||||
// draw2dTargetsPipe.setParams(draw2DTargetsParams);
|
||||
//
|
||||
// var draw2dCrosshairParams =
|
||||
// new Draw2dCrosshairPipe.Draw2dCrosshairParams(
|
||||
// settings.outputShouldDraw,
|
||||
// settings.offsetRobotOffsetMode,
|
||||
// settings.offsetSinglePoint,
|
||||
// dualOffsetValues,
|
||||
// frameStaticProperties);
|
||||
// draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
|
||||
//
|
||||
// var draw3dTargetsParams =
|
||||
// new Draw3dTargetsPipe.Draw3dContoursParams(
|
||||
// settings.outputShouldDraw,
|
||||
// frameStaticProperties.cameraCalibration,
|
||||
// settings.targetModel);
|
||||
// draw3dTargetsPipe.setParams(draw3dTargetsParams);
|
||||
|
||||
var solvePNPParams =
|
||||
new SolvePNPPipe.SolvePNPPipeParams(
|
||||
@@ -167,7 +163,6 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
|
||||
|
||||
@Override
|
||||
public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings) {
|
||||
setPipeParams(frame.frameStaticProperties, settings);
|
||||
|
||||
long sumPipeNanosElapsed = 0L;
|
||||
|
||||
@@ -228,44 +223,51 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
|
||||
targetList = collect2dTargetsResult.output;
|
||||
}
|
||||
|
||||
var fpsResult = calculateFPSPipe.run(null);
|
||||
var fps = fpsResult.output;
|
||||
sumPipeNanosElapsed += fpsResult.nanosElapsed;
|
||||
// var fpsResult = calculateFPSPipe.run(null);
|
||||
// var fps = fpsResult.output;
|
||||
// sumPipeNanosElapsed += fpsResult.nanosElapsed;
|
||||
|
||||
// Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
|
||||
var outputMatPipeResult = outputMatPipe.run(hsvPipeResult.output);
|
||||
sumPipeNanosElapsed += pipeProfileNanos[12] = outputMatPipeResult.nanosElapsed;
|
||||
|
||||
// Draw 2D Crosshair on input and output
|
||||
var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(rawInputMat, targetList));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[13] = draw2dCrosshairResultOnInput.nanosElapsed;
|
||||
|
||||
var draw2dCrosshairResultOnOutput =
|
||||
draw2dCrosshairPipe.run(Pair.of(hsvPipeResult.output, targetList));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[14] = draw2dCrosshairResultOnOutput.nanosElapsed;
|
||||
|
||||
// Draw 2D contours on input and output
|
||||
var draw2dTargetsOnInput =
|
||||
draw2dTargetsPipe.run(Triple.of(rawInputMat, collect2dTargetsResult.output, fps));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[15] = draw2dTargetsOnInput.nanosElapsed;
|
||||
|
||||
var draw2dTargetsOnOutput =
|
||||
draw2dTargetsPipe.run(Triple.of(hsvPipeResult.output, collect2dTargetsResult.output, fps));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[16] = draw2dTargetsOnOutput.nanosElapsed;
|
||||
|
||||
// Draw 3D Targets on input and output if necessary
|
||||
if (settings.solvePNPEnabled) {
|
||||
var drawOnInputResult =
|
||||
draw3dTargetsPipe.run(Pair.of(rawInputMat, collect2dTargetsResult.output));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[17] = drawOnInputResult.nanosElapsed;
|
||||
|
||||
var drawOnOutputResult =
|
||||
draw3dTargetsPipe.run(Pair.of(hsvPipeResult.output, collect2dTargetsResult.output));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[18] = drawOnOutputResult.nanosElapsed;
|
||||
} else {
|
||||
pipeProfileNanos[17] = 0;
|
||||
pipeProfileNanos[18] = 0;
|
||||
}
|
||||
// // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
|
||||
// var outputMatPipeResult = outputMatPipe.run(hsvPipeResult.output);
|
||||
// sumPipeNanosElapsed += pipeProfileNanos[12] = outputMatPipeResult.nanosElapsed;
|
||||
//
|
||||
// // Draw 2D Crosshair on input and output
|
||||
// var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(rawInputMat,
|
||||
// targetList));
|
||||
// sumPipeNanosElapsed += pipeProfileNanos[13] =
|
||||
// draw2dCrosshairResultOnInput.nanosElapsed;
|
||||
//
|
||||
// var draw2dCrosshairResultOnOutput =
|
||||
// draw2dCrosshairPipe.run(Pair.of(hsvPipeResult.output, targetList));
|
||||
// sumPipeNanosElapsed += pipeProfileNanos[14] =
|
||||
// draw2dCrosshairResultOnOutput.nanosElapsed;
|
||||
//
|
||||
// // Draw 2D contours on input and output
|
||||
// var draw2dTargetsOnInput =
|
||||
// draw2dTargetsPipe.run(Triple.of(rawInputMat, collect2dTargetsResult.output,
|
||||
// fps));
|
||||
// sumPipeNanosElapsed += pipeProfileNanos[15] = draw2dTargetsOnInput.nanosElapsed;
|
||||
//
|
||||
// var draw2dTargetsOnOutput =
|
||||
// draw2dTargetsPipe.run(Triple.of(hsvPipeResult.output,
|
||||
// collect2dTargetsResult.output, fps));
|
||||
// sumPipeNanosElapsed += pipeProfileNanos[16] = draw2dTargetsOnOutput.nanosElapsed;
|
||||
//
|
||||
// // Draw 3D Targets on input and output if necessary
|
||||
// if (settings.solvePNPEnabled) {
|
||||
// var drawOnInputResult =
|
||||
// draw3dTargetsPipe.run(Pair.of(rawInputMat,
|
||||
// collect2dTargetsResult.output));
|
||||
// sumPipeNanosElapsed += pipeProfileNanos[17] = drawOnInputResult.nanosElapsed;
|
||||
//
|
||||
// var drawOnOutputResult =
|
||||
// draw3dTargetsPipe.run(Pair.of(hsvPipeResult.output,
|
||||
// collect2dTargetsResult.output));
|
||||
// sumPipeNanosElapsed += pipeProfileNanos[18] = drawOnOutputResult.nanosElapsed;
|
||||
// } else {
|
||||
// pipeProfileNanos[17] = 0;
|
||||
// pipeProfileNanos[18] = 0;
|
||||
// }
|
||||
|
||||
PipelineProfiler.printReflectiveProfile(pipeProfileNanos);
|
||||
|
||||
|
||||
@@ -18,67 +18,11 @@
|
||||
package org.photonvision.vision.pipeline;
|
||||
|
||||
import com.fasterxml.jackson.annotation.JsonTypeName;
|
||||
import java.util.Objects;
|
||||
import org.photonvision.vision.opencv.ContourGroupingMode;
|
||||
import org.photonvision.vision.opencv.ContourIntersectionDirection;
|
||||
import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
|
||||
import org.photonvision.vision.target.TargetModel;
|
||||
|
||||
@JsonTypeName("ReflectivePipelineSettings")
|
||||
public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
|
||||
// how many contours to attempt to group (Single, Dual)
|
||||
public ContourGroupingMode contourGroupingMode = ContourGroupingMode.Single;
|
||||
|
||||
// the direction in which contours must intersect to be considered intersecting
|
||||
public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;
|
||||
|
||||
// 3d settings
|
||||
public boolean solvePNPEnabled = false;
|
||||
public TargetModel targetModel = TargetModel.get2020Target();
|
||||
|
||||
// Corner detection settings
|
||||
public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
|
||||
CornerDetectionPipe.DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS;
|
||||
public boolean cornerDetectionUseConvexHulls = true;
|
||||
public boolean cornerDetectionExactSideCount = false;
|
||||
public int cornerDetectionSideCount = 4;
|
||||
public double cornerDetectionAccuracyPercentage = 10;
|
||||
|
||||
public ReflectivePipelineSettings() {
|
||||
super();
|
||||
pipelineType = PipelineType.Reflective;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean equals(Object o) {
|
||||
if (this == o) return true;
|
||||
if (o == null || getClass() != o.getClass()) return false;
|
||||
if (!super.equals(o)) return false;
|
||||
ReflectivePipelineSettings that = (ReflectivePipelineSettings) o;
|
||||
return solvePNPEnabled == that.solvePNPEnabled
|
||||
&& cornerDetectionUseConvexHulls == that.cornerDetectionUseConvexHulls
|
||||
&& cornerDetectionExactSideCount == that.cornerDetectionExactSideCount
|
||||
&& cornerDetectionSideCount == that.cornerDetectionSideCount
|
||||
&& Double.compare(that.cornerDetectionAccuracyPercentage, cornerDetectionAccuracyPercentage)
|
||||
== 0
|
||||
&& contourGroupingMode == that.contourGroupingMode
|
||||
&& contourIntersection == that.contourIntersection
|
||||
&& targetModel.equals(that.targetModel)
|
||||
&& cornerDetectionStrategy == that.cornerDetectionStrategy;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(
|
||||
super.hashCode(),
|
||||
contourGroupingMode,
|
||||
contourIntersection,
|
||||
solvePNPEnabled,
|
||||
targetModel,
|
||||
cornerDetectionStrategy,
|
||||
cornerDetectionUseConvexHulls,
|
||||
cornerDetectionExactSideCount,
|
||||
cornerDetectionSideCount,
|
||||
cornerDetectionAccuracyPercentage);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -35,8 +35,8 @@ public class CVPipelineResult implements Releasable {
|
||||
this.processingMillis = processingMillis;
|
||||
this.targets = targets != null ? targets : Collections.emptyList();
|
||||
|
||||
this.outputFrame = Frame.copyFromAndRelease(outputFrame);
|
||||
this.inputFrame = inputFrame != null ? Frame.copyFromAndRelease(inputFrame) : null;
|
||||
this.outputFrame = outputFrame;
|
||||
this.inputFrame = inputFrame;
|
||||
}
|
||||
|
||||
public CVPipelineResult(double processingMillis, List<TrackedTarget> targets, Frame outputFrame) {
|
||||
|
||||
@@ -37,7 +37,10 @@ import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
|
||||
import org.photonvision.vision.camera.CameraQuirk;
|
||||
import org.photonvision.vision.camera.QuirkyCamera;
|
||||
import org.photonvision.vision.camera.USBCameraSource;
|
||||
import org.photonvision.vision.frame.Frame;
|
||||
import org.photonvision.vision.frame.consumer.MJPGFrameConsumer;
|
||||
import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
|
||||
import org.photonvision.vision.pipeline.OutputStreamPipeline;
|
||||
import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
|
||||
import org.photonvision.vision.pipeline.UICalibrationData;
|
||||
import org.photonvision.vision.pipeline.result.CVPipelineResult;
|
||||
@@ -58,6 +61,7 @@ public class VisionModule {
|
||||
protected final PipelineManager pipelineManager;
|
||||
protected final VisionSource visionSource;
|
||||
private final VisionRunner visionRunner;
|
||||
private final StreamRunnable streamRunnable;
|
||||
private final LinkedList<CVPipelineResultConsumer> resultConsumers = new LinkedList<>();
|
||||
private final LinkedList<CVPipelineResultConsumer> fpsLimitedResultConsumers = new LinkedList<>();
|
||||
private final NTDataPublisher ntConsumer;
|
||||
@@ -84,6 +88,7 @@ public class VisionModule {
|
||||
this.visionSource.getFrameProvider(),
|
||||
this.pipelineManager::getCurrentUserPipeline,
|
||||
this::consumeResult);
|
||||
this.streamRunnable = new StreamRunnable(new OutputStreamPipeline());
|
||||
this.moduleIndex = index;
|
||||
|
||||
// do this
|
||||
@@ -140,6 +145,58 @@ public class VisionModule {
|
||||
saveAndBroadcastAll();
|
||||
}
|
||||
|
||||
private class StreamRunnable extends Thread {
|
||||
private final OutputStreamPipeline outputStreamPipeline;
|
||||
|
||||
private Frame inputFrame, outputFrame;
|
||||
private AdvancedPipelineSettings settings = new AdvancedPipelineSettings();
|
||||
private List<TrackedTarget> targets = new ArrayList<>();
|
||||
private double fps = 0;
|
||||
|
||||
private boolean shouldRun = false;
|
||||
|
||||
public StreamRunnable(OutputStreamPipeline outputStreamPipeline) {
|
||||
this.outputStreamPipeline = outputStreamPipeline;
|
||||
}
|
||||
|
||||
public void updateData(
|
||||
Frame inputFrame,
|
||||
Frame outputFrame,
|
||||
AdvancedPipelineSettings settings,
|
||||
List<TrackedTarget> targets,
|
||||
double fps) {
|
||||
this.inputFrame = inputFrame;
|
||||
this.outputFrame = outputFrame;
|
||||
this.settings = settings;
|
||||
this.targets = targets;
|
||||
this.fps = fps;
|
||||
|
||||
shouldRun =
|
||||
inputFrame != null
|
||||
&& !inputFrame.image.getMat().empty()
|
||||
&& outputFrame != null
|
||||
&& !outputFrame.image.getMat().empty();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
while (true) {
|
||||
if (shouldRun) {
|
||||
var osr = outputStreamPipeline.process(inputFrame, outputFrame, settings, targets, fps);
|
||||
consumeFpsLimitedResult(osr);
|
||||
shouldRun = false;
|
||||
} else {
|
||||
// busy wait! hurray!
|
||||
try {
|
||||
Thread.sleep(1);
|
||||
} catch (InterruptedException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void setDriverMode(boolean isDriverMode) {
|
||||
pipelineManager.setDriverMode(isDriverMode);
|
||||
setVisionLEDs(!isDriverMode);
|
||||
@@ -148,6 +205,7 @@ public class VisionModule {
|
||||
|
||||
public void start() {
|
||||
visionRunner.startProcess();
|
||||
streamRunnable.start();
|
||||
}
|
||||
|
||||
public void setFovAndPitch(double fov, Rotation2d pitch) {
|
||||
@@ -347,9 +405,21 @@ public class VisionModule {
|
||||
|
||||
private void consumeResult(CVPipelineResult result) {
|
||||
consumePipelineResult(result);
|
||||
consumeFpsLimitedResult(result);
|
||||
|
||||
result.release();
|
||||
// total hack. kms
|
||||
if (pipelineManager.getCurrentPipelineIndex() >= 0) {
|
||||
var fps = 1000.0 / result.getLatencyMillis();
|
||||
streamRunnable.updateData(
|
||||
result.inputFrame,
|
||||
result.outputFrame,
|
||||
(AdvancedPipelineSettings) pipelineManager.getCurrentPipelineSettings(),
|
||||
result.targets,
|
||||
fps);
|
||||
// the streamRunnable manages releasing in this case
|
||||
} else {
|
||||
consumeFpsLimitedResult(result);
|
||||
result.release();
|
||||
}
|
||||
}
|
||||
|
||||
private void consumePipelineResult(CVPipelineResult result) {
|
||||
@@ -365,6 +435,7 @@ public class VisionModule {
|
||||
}
|
||||
lastFrameConsumeMillis = System.currentTimeMillis();
|
||||
}
|
||||
result.release();
|
||||
}
|
||||
|
||||
public void setTargetModel(TargetModel targetModel) {
|
||||
|
||||
Reference in New Issue
Block a user