Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Resize image before drawing (#193)

Browse Source 
This helps line viability at high divisors
This commit is contained in:
Matt
2020-12-27 14:07:53 -08:00
committed by GitHub
parent 7a9d999c15
commit 79fc194575
10 changed files with 134 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
photon-server/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java
View File

@@ -26,9 +26,7 @@ import edu.wpi.cscore.VideoMode;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.ArrayList;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameDivisor;
@@ -36,7 +34,6 @@ public class MJPGFrameConsumer {
private CvSource cvSource;
private MjpegServer mjpegServer;
private FrameDivisor divisor = FrameDivisor.NONE;
@SuppressWarnings("FieldCanBeLocal")
private VideoListener listener;
@@ -97,21 +94,9 @@ public class MJPGFrameConsumer {
this(name, 320, 240, port);
}
public void setFrameDivisor(FrameDivisor divisor) {
this.divisor = divisor;
}
public void accept(Frame frame) {
if (frame != null && !frame.image.getMat().empty()) {
if (divisor != FrameDivisor.NONE) {
var tempMat = new Mat();
Imgproc.resize(
frame.image.getMat(), tempMat, getScaledSize(frame.image.getMat().size(), divisor));
cvSource.putFrame(tempMat);
tempMat.release();
} else {
cvSource.putFrame(frame.image.getMat());
}
cvSource.putFrame(frame.image.getMat());
}
}

16
photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java
View File

@@ -24,6 +24,7 @@ import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.vision.frame.FrameDivisor;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.DualOffsetValues;
import org.photonvision.vision.pipe.MutatingPipe;
@@ -39,13 +40,12 @@ public class Draw2dCrosshairPipe
protected Void process(Pair<Mat, List<TrackedTarget>> in) {
if (!params.shouldDraw) return null;
var camCenterPoint = params.frameStaticProperties.centerPoint;
var image = in.getLeft();
if (params.showCrosshair) {
double x = params.frameStaticProperties.centerX;
double y = params.frameStaticProperties.centerY;
double scale = params.frameStaticProperties.imageWidth / 32.0;
double scale = params.frameStaticProperties.imageWidth / (double) params.divisor.value / 32.0;
switch (params.robotOffsetPointMode) {
case Single:
@@ -68,6 +68,9 @@ public class Draw2dCrosshairPipe
break;
}
x /= (double) params.divisor.value;
y /= (double) params.divisor.value;
Point xMax = new Point(x + scale, y);
Point xMin = new Point(x - scale, y);
Point yMax = new Point(x, y + scale);
@@ -88,13 +91,16 @@ public class Draw2dCrosshairPipe
public final RobotOffsetPointMode robotOffsetPointMode;
public final Point singleOffsetPoint;
public final DualOffsetValues dualOffsetValues;
private final FrameDivisor divisor;
public Draw2dCrosshairParams(FrameStaticProperties frameStaticProperties) {
public Draw2dCrosshairParams(
FrameStaticProperties frameStaticProperties, FrameDivisor divisor) {
shouldDraw = true;
this.frameStaticProperties = frameStaticProperties;
robotOffsetPointMode = RobotOffsetPointMode.None;
singleOffsetPoint = new Point();
dualOffsetValues = new DualOffsetValues();
this.divisor = divisor;
}
public Draw2dCrosshairParams(
@@ -102,12 +108,14 @@ public class Draw2dCrosshairPipe
RobotOffsetPointMode robotOffsetPointMode,
Point singleOffsetPoint,
DualOffsetValues dualOffsetValues,
FrameStaticProperties frameStaticProperties) {
FrameStaticProperties frameStaticProperties,
FrameDivisor divisor) {
this.shouldDraw = shouldDraw;
this.frameStaticProperties = frameStaticProperties;
this.robotOffsetPointMode = robotOffsetPointMode;
this.singleOffsetPoint = singleOffsetPoint;
this.dualOffsetValues = dualOffsetValues;
this.divisor = divisor;
}
}
}

58
photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java
View File

@@ -18,20 +18,23 @@
package org.photonvision.vision.pipe.impl;
import java.awt.*;
import java.util.ArrayList;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.*;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.vision.frame.FrameDivisor;
import org.photonvision.vision.pipe.MutatingPipe;
import org.photonvision.vision.target.TrackedTarget;
public class Draw2dTargetsPipe
extends MutatingPipe<Pair<Mat, List<TrackedTarget>>, Draw2dTargetsPipe.Draw2dTargetsParams> {
private List<MatOfPoint> m_drawnContours = new ArrayList<>();
MatOfPoint tempMat = new MatOfPoint();
private static final Logger logger = new Logger(Draw2dTargetsPipe.class, LogGroup.General);
@Override
protected Void process(Pair<Mat, List<TrackedTarget>> in) {
@@ -65,18 +68,14 @@ public class Draw2dTargetsPipe
if (r == null) continue;
m_drawnContours.forEach(Mat::release);
m_drawnContours.clear();
m_drawnContours = new ArrayList<>();
r.points(vertices);
dividePointArray(vertices);
contour.fromArray(vertices);
m_drawnContours.add(contour);
if (params.showRotatedBox) {
Imgproc.drawContours(
in.getLeft(),
m_drawnContours,
List.of(contour),
0,
rotatedBoxColour,
(int) Math.ceil(imageSize * params.kPixelsToBoxThickness));
@@ -93,9 +92,10 @@ public class Draw2dTargetsPipe
}
if (params.showShape) {
divideMat(target.m_mainContour.mat, tempMat);
Imgproc.drawContours(
in.getLeft(),
List.of(target.m_mainContour.mat),
List.of(tempMat),
-1,
shapeColour,
(int) Math.ceil(imageSize * params.kPixelsToBoxThickness));
@@ -107,6 +107,7 @@ public class Draw2dTargetsPipe
new Point(
center.x + params.kPixelsToOffset * imageSize,
center.y - params.kPixelsToOffset * imageSize);
dividePoint(textPos);
Imgproc.putText(
in.getLeft(),
@@ -120,7 +121,8 @@ public class Draw2dTargetsPipe
if (params.showCentroid) {
Point centroid = target.getTargetOffsetPoint();
Point centroid = target.getTargetOffsetPoint().clone();
dividePoint(centroid);
var crosshairRadius = (int) (imageSize * params.kPixelsToCentroidRadius);
var x = centroid.x;
var y = centroid.y;
@@ -148,10 +150,37 @@ public class Draw2dTargetsPipe
return null;
}
private void divideMat(MatOfPoint src, MatOfPoint dst) {
var hull = src.toArray();
for (Point point : hull) {
dividePoint(point);
}
dst.fromArray(hull);
}
/** Scale a given point list by the current frame divisor. the point list is mutated! */
private void dividePointList(List<Point> points) {
for (var p : points) {
dividePoint(p);
}
}
/** Scale a given point array by the current frame divisor. the point list is mutated! */
private void dividePointArray(Point[] points) {
for (var p : points) {
dividePoint(p);
}
}
private void dividePoint(Point p) {
p.x = p.x / (double) params.divisor.value;
p.y = p.y / (double) params.divisor.value;
}
public static class Draw2dTargetsParams {
public double kPixelsToText = 0.0025;
public double kPixelsToThickness = 0.008;
public double kPixelsToOffset = 0.02;
public double kPixelsToOffset = 0.04;
public double kPixelsToBoxThickness = 0.007;
public double kPixelsToCentroidRadius = 0.03;
public boolean showCentroid = true;
@@ -168,10 +197,13 @@ public class Draw2dTargetsPipe
public final boolean showMultipleTargets;
public final boolean shouldDraw;
// TODO: set other params from UI/settings file?
public Draw2dTargetsParams(boolean shouldDraw, boolean showMultipleTargets) {
public final FrameDivisor divisor;
public Draw2dTargetsParams(
boolean shouldDraw, boolean showMultipleTargets, FrameDivisor divisor) {
this.shouldDraw = shouldDraw;
this.showMultipleTargets = showMultipleTargets;
this.divisor = divisor;
}
}
}

43
photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java
View File

@@ -21,15 +21,16 @@ import java.awt.*;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.FrameDivisor;
import org.photonvision.vision.pipe.MutatingPipe;
import org.photonvision.vision.target.TargetModel;
import org.photonvision.vision.target.TrackedTarget;
@@ -47,7 +48,7 @@ public class Draw3dTargetsPipe
// draw convex hull
var pointMat = new MatOfPoint();
target.m_mainContour.getConvexHull().convertTo(pointMat, CvType.CV_32S);
divideMat2f(target.m_mainContour.getConvexHull(), pointMat);
if (pointMat.size().empty()) {
logger.error("Convex hull is empty?");
logger.debug(
@@ -60,7 +61,7 @@ public class Draw3dTargetsPipe
// draw approximate polygon
var poly = target.getApproximateBoundingPolygon();
if (poly != null) {
poly.convertTo(pointMat, CvType.CV_32S);
divideMat2f(poly, pointMat);
Imgproc.drawContours(
in.getLeft(), List.of(pointMat), -1, ColorHelper.colorToScalar(Color.blue), 2);
}
@@ -89,6 +90,10 @@ public class Draw3dTargetsPipe
tempMat,
jac);
var topPoints = tempMat.toList();
dividePointList(bottomPoints);
dividePointList(topPoints);
// floor, then pillers, then top
for (int i = 0; i < bottomPoints.size(); i++) {
Imgproc.line(
@@ -114,7 +119,7 @@ public class Draw3dTargetsPipe
ColorHelper.colorToScalar(Color.orange),
3);
}
tempMat.release();
jac.release();
}
pointMat.release();
@@ -123,9 +128,12 @@ public class Draw3dTargetsPipe
var corners = target.getTargetCorners();
if (corners != null && !corners.isEmpty()) {
for (var corner : corners) {
var x = corner.x / (double) params.divisor.value;
var y = corner.y / (double) params.divisor.value;
Imgproc.circle(
in.getLeft(),
corner,
new Point(x, y),
params.radius,
ColorHelper.colorToScalar(params.color),
params.radius);
@@ -136,6 +144,26 @@ public class Draw3dTargetsPipe
return null;
}
private void divideMat2f(MatOfPoint2f src, MatOfPoint dst) {
var hull = src.toArray();
var pointArray = new Point[hull.length];
for (int i = 0; i < hull.length; i++) {
var hullAtI = hull[i];
pointArray[i] =
new Point(
hullAtI.x / (double) params.divisor.value, hullAtI.y / (double) params.divisor.value);
}
dst.fromArray(pointArray);
}
/** Scale a given point list by the current frame divisor. the point list is mutated! */
private void dividePointList(List<Point> points) {
for (var p : points) {
p.x = p.x / (double) params.divisor.value;
p.y = p.y / (double) params.divisor.value;
}
}
public static class Draw3dContoursParams {
public int radius = 2;
public Color color = Color.RED;
@@ -143,14 +171,17 @@ public class Draw3dTargetsPipe
public final boolean shouldDraw;
public final TargetModel targetModel;
public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
public final FrameDivisor divisor;
public Draw3dContoursParams(
boolean shouldDraw,
CameraCalibrationCoefficients cameraCalibrationCoefficients,
TargetModel targetModel) {
TargetModel targetModel,
FrameDivisor divisor) {
this.shouldDraw = shouldDraw;
this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
this.targetModel = targetModel;
this.divisor = divisor;
}
}
}

28
photon-server/src/main/java/org/photonvision/vision/pipe/impl/ResizeImagePipe.java
View File

@@ -26,10 +26,6 @@ import org.photonvision.vision.pipe.MutatingPipe;
/** Pipe that resizes an image to a given resolution */
public class ResizeImagePipe extends MutatingPipe<Mat, ResizeImagePipe.ResizeImageParams> {
public ResizeImagePipe() {
setParams(ResizeImageParams.DEFAULT);
}
/**
* Process this pipe
*
@@ -37,36 +33,20 @@ public class ResizeImagePipe extends MutatingPipe<Mat, ResizeImagePipe.ResizeIma
*/
@Override
protected Void process(Mat in) {
int width = in.cols() / params.getDivisor().value;
int height = in.rows() / params.getDivisor().value;
Imgproc.resize(in, in, new Size(width, height));
// if a divisor is set, use that instead of a size.
if (params.getDivisor() != null) {
int width = in.cols() / params.getDivisor().value;
int height = in.rows() / params.getDivisor().value;
setParams(new ResizeImageParams(width, height));
}
Imgproc.resize(in, in, params.getSize());
return null;
}
public static class ResizeImageParams {
public static ResizeImageParams DEFAULT = new ResizeImageParams(320, 240);
private Size size;
private FrameDivisor divisor;
public ResizeImageParams(int width, int height) {
size = new Size(new double[] {width, height});
}
private final FrameDivisor divisor;
public ResizeImageParams(FrameDivisor divisor) {
this.divisor = divisor;
}
public Size getSize() {
return size;
}
public FrameDivisor getDivisor() {
return divisor;
}

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

@@ -165,7 +165,9 @@ public class ColoredShapePipeline
Draw2dTargetsPipe.Draw2dTargetsParams draw2DTargetsParams =
new Draw2dTargetsPipe.Draw2dTargetsParams(
settings.outputShouldDraw, settings.outputShowMultipleTargets);
settings.outputShouldDraw,
settings.outputShowMultipleTargets,
settings.streamingFrameDivisor);
draw2DTargetsParams.showShape = true;
draw2DTargetsParams.showMaximumBox = false;
draw2DTargetsParams.showRotatedBox = false;
@@ -177,14 +179,16 @@ public class ColoredShapePipeline
settings.offsetRobotOffsetMode,
settings.offsetSinglePoint,
dualOffsetValues,
frameStaticProperties);
frameStaticProperties,
settings.streamingFrameDivisor);
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
var draw3dTargetsParams =
new Draw3dTargetsPipe.Draw3dContoursParams(
settings.outputShouldDraw,
frameStaticProperties.cameraCalibration,
settings.targetModel);
settings.targetModel,
settings.streamingFrameDivisor);
draw3dTargetsPipe.setParams(draw3dTargetsParams);
}

3
photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java
View File

@@ -49,7 +49,8 @@ public class DriverModePipeline
rotateImagePipe.setParams(rotateImageParams);
Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
new Draw2dCrosshairPipe.Draw2dCrosshairParams(frameStaticProperties);
new Draw2dCrosshairPipe.Draw2dCrosshairParams(
frameStaticProperties, settings.streamingFrameDivisor);
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
if (PicamJNI.isSupported()) {

40
photon-server/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java
View File

@@ -39,8 +39,9 @@ public class OutputStreamPipeline {
private final Draw2dTargetsPipe draw2dTargetsPipe = new Draw2dTargetsPipe();
private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
private final long[] pipeProfileNanos = new long[10];
private final long[] pipeProfileNanos = new long[12];
protected void setPipeParams(
FrameStaticProperties frameStaticProperties, AdvancedPipelineSettings settings) {
@@ -54,7 +55,9 @@ public class OutputStreamPipeline {
var draw2DTargetsParams =
new Draw2dTargetsPipe.Draw2dTargetsParams(
settings.outputShouldDraw, settings.outputShowMultipleTargets);
settings.outputShouldDraw,
settings.outputShowMultipleTargets,
settings.streamingFrameDivisor);
draw2dTargetsPipe.setParams(draw2DTargetsParams);
var draw2dCrosshairParams =
@@ -63,15 +66,20 @@ public class OutputStreamPipeline {
settings.offsetRobotOffsetMode,
settings.offsetSinglePoint,
dualOffsetValues,
frameStaticProperties);
frameStaticProperties,
settings.streamingFrameDivisor);
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
var draw3dTargetsParams =
new Draw3dTargetsPipe.Draw3dContoursParams(
settings.outputShouldDraw,
frameStaticProperties.cameraCalibration,
settings.targetModel);
settings.targetModel,
settings.streamingFrameDivisor);
draw3dTargetsPipe.setParams(draw3dTargetsParams);
resizeImagePipe.setParams(
new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
}
public CVPipelineResult process(
@@ -85,34 +93,38 @@ public class OutputStreamPipeline {
long sumPipeNanosElapsed = 0L;
// Resize both in place before doing any conversion
sumPipeNanosElapsed += pipeProfileNanos[0] = resizeImagePipe.run(inMat).nanosElapsed;
sumPipeNanosElapsed += pipeProfileNanos[1] = resizeImagePipe.run(outMat).nanosElapsed;
// Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
var outputMatPipeResult = outputMatPipe.run(outMat);
sumPipeNanosElapsed += pipeProfileNanos[0] = outputMatPipeResult.nanosElapsed;
sumPipeNanosElapsed += pipeProfileNanos[2] = outputMatPipeResult.nanosElapsed;
// Draw 2D Crosshair on input and output
var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
sumPipeNanosElapsed += pipeProfileNanos[1] = draw2dCrosshairResultOnInput.nanosElapsed;
sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
sumPipeNanosElapsed += pipeProfileNanos[2] = draw2dCrosshairResultOnOutput.nanosElapsed;
var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
// Draw 2D contours on input and output
var draw2dTargetsOnInput = draw2dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dTargetsOnInput.nanosElapsed;
sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dTargetsOnOutput.nanosElapsed;
sumPipeNanosElapsed += pipeProfileNanos[6] = 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;
sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
sumPipeNanosElapsed += pipeProfileNanos[6] = drawOnOutputResult.nanosElapsed;
sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
} else {
pipeProfileNanos[5] = 0;
pipeProfileNanos[6] = 0;
pipeProfileNanos[7] = 0;
pipeProfileNanos[8] = 0;
}
var fpsResult = calculateFPSPipe.run(null);

11
photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
View File

@@ -127,11 +127,6 @@ public class VisionModule {
setPipeline(visionSource.getSettables().getConfiguration().currentPipelineIndex);
dashboardInputStreamer.setFrameDivisor(
pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor);
dashboardOutputStreamer.setFrameDivisor(
pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor);
// Set vendor FOV
if (isVendorCamera()) {
var fov = ConfigManager.getInstance().getConfig().getHardwareConfig().vendorFOV;
@@ -367,12 +362,6 @@ public class VisionModule {
visionSource.getSettables().setBrightness(config.cameraBrightness);
visionSource.getSettables().setExposure(config.cameraExposure);
// Also set new frame divisor
dashboardInputStreamer.setFrameDivisor(
pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor);
dashboardOutputStreamer.setFrameDivisor(
pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor);
if (!cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
config.cameraGain = -1;
} else {

9
photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java
View File

@@ -198,15 +198,6 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
propField.set(newPropValue, newPropValue);
}
logger.trace("Set prop " + propName + " to value " + newPropValue);
// special case for extra tasks to perform after setting PipelineSettings
if (propName.equals("streamingFrameDivisor")) {
parentModule.dashboardInputStreamer.setFrameDivisor(
parentModule.pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor);
parentModule.dashboardOutputStreamer.setFrameDivisor(
parentModule.pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor);
}
} catch (NoSuchFieldException | IllegalAccessException e) {
logger.error(
"Could not set prop "