From 79fc19457598f29fcdc7d532b12a935a75d34a0d Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 27 Dec 2020 14:07:53 -0800 Subject: [PATCH] Resize image before drawing (#193) This helps line viability at high divisors --- .../frame/consumer/MJPGFrameConsumer.java | 17 +----- .../vision/pipe/impl/Draw2dCrosshairPipe.java | 16 +++-- .../vision/pipe/impl/Draw2dTargetsPipe.java | 58 ++++++++++++++----- .../vision/pipe/impl/Draw3dTargetsPipe.java | 43 ++++++++++++-- .../vision/pipe/impl/ResizeImagePipe.java | 28 ++------- .../vision/pipeline/ColoredShapePipeline.java | 10 +++- .../vision/pipeline/DriverModePipeline.java | 3 +- .../vision/pipeline/OutputStreamPipeline.java | 40 ++++++++----- .../vision/processes/VisionModule.java | 11 ---- .../VisionModuleChangeSubscriber.java | 9 --- 10 files changed, 134 insertions(+), 101 deletions(-) diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java b/photon-server/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java index ee34ee718..5c3c6a3c5 100644 --- a/photon-server/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java +++ b/photon-server/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java @@ -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()); } } 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 e4d94d184..6d1d2bb89 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 @@ -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> 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; } } } 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 7ab74330e..d1d79183e 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 @@ -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>, Draw2dTargetsPipe.Draw2dTargetsParams> { - private List m_drawnContours = new ArrayList<>(); + MatOfPoint tempMat = new MatOfPoint(); + private static final Logger logger = new Logger(Draw2dTargetsPipe.class, LogGroup.General); @Override protected Void process(Pair> 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 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; } } } 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 e25896fab..7a3fb5682 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 @@ -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 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; } } } 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 c7a83c3bd..9e207a243 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 @@ -26,10 +26,6 @@ import org.photonvision.vision.pipe.MutatingPipe; /** Pipe that resizes an image to a given resolution */ public class ResizeImagePipe extends MutatingPipe { - public ResizeImagePipe() { - setParams(ResizeImageParams.DEFAULT); - } - /** * Process this pipe * @@ -37,36 +33,20 @@ public class ResizeImagePipe extends MutatingPipe