From e863cc07810262b5a394d8c9204db01a9fbf8434 Mon Sep 17 00:00:00 2001 From: Matt Date: Sat, 2 Nov 2019 12:38:42 -0700 Subject: [PATCH] Move pipeline logic into CVProcess --- .../vision/process/CVProcess.java | 99 ++++++++++++++++++- .../vision/process/CameraProcess.java | 3 + .../vision/process/VisionProcess.java | 91 ++--------------- 3 files changed, 107 insertions(+), 86 deletions(-) diff --git a/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java index 51256291a..f99d26657 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java @@ -1,8 +1,6 @@ package com.chameleonvision.vision.process; -import com.chameleonvision.vision.SortMode; -import com.chameleonvision.vision.TargetGroup; -import com.chameleonvision.vision.TargetIntersection; +import com.chameleonvision.vision.*; import com.chameleonvision.vision.camera.CameraValues; import com.chameleonvision.util.MathHandler; import org.apache.commons.math3.util.FastMath; @@ -34,6 +32,101 @@ public class CVProcess { this.cameraValues = cameraValues; } + private Mat cameraInputMat = new Mat(); + private Mat hsvThreshMat = new Mat(); + private Mat streamOutputMat = new Mat(); + + private List foundContours_ = new ArrayList<>(); + private List filteredContours_ = new ArrayList<>(); + private List deSpeckledContours_ = new ArrayList<>(); + private List groupedContours_ = new ArrayList<>(); + + private static final Scalar contourRectColor = new Scalar(255, 0, 0); + private static final Scalar BoxRectColor = new Scalar(0, 0, 233); + + private void drawContour(Mat inputMat, RotatedRect contourRect) { + if (contourRect == null) return; + List drawnContour = new ArrayList<>(); + Point[] vertices = new Point[4]; + contourRect.points(vertices); + MatOfPoint contour = new MatOfPoint(vertices); + drawnContour.add(contour); + Rect box = Imgproc.boundingRect(contour); + Imgproc.drawContours(inputMat, drawnContour, 0, contourRectColor, 3); + Imgproc.circle(inputMat, contourRect.center, 3, contourRectColor); + Imgproc.rectangle(inputMat, new Point(box.x, box.y), new Point((box.x + box.width), (box.y + box.height)), BoxRectColor, 2); + } + + PipelineResult runPipeline(Pipeline currentPipeline, Mat inputImage, Mat outputImage, CameraValues cameraValues, boolean shouldFlip, boolean driverMode) { + var pipelineResult = new PipelineResult(); + + if (currentPipeline == null) { + return pipelineResult; + } + if (shouldFlip) { + Core.flip(inputImage, inputImage, -1); + } + if (driverMode) { + inputImage.copyTo(outputImage); + return pipelineResult; + } + + foundContours_.clear(); + filteredContours_.clear(); + deSpeckledContours_.clear(); + groupedContours_.clear(); + + Scalar hsvLower = new Scalar(currentPipeline.hue.get(0).intValue(), currentPipeline.saturation.get(0).intValue(), currentPipeline.value.get(0).intValue()); + Scalar hsvUpper = new Scalar(currentPipeline.hue.get(1).intValue(), currentPipeline.saturation.get(1).intValue(), currentPipeline.value.get(1).intValue()); + + hsvThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate); + + if (currentPipeline.isBinary) { + Imgproc.cvtColor(hsvThreshMat, outputImage, Imgproc.COLOR_GRAY2BGR, 3); + } else { + inputImage.copyTo(outputImage); + } + foundContours_ = findContours(hsvThreshMat); + if (foundContours_.size() > 0) { + filteredContours_ = filterContours(foundContours_, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent); + if (filteredContours_.size() > 0) { + deSpeckledContours_ = rejectSpeckles(filteredContours_, currentPipeline.speckle.doubleValue()); + if (deSpeckledContours_.size() > 0) { + groupedContours_ = groupTargets(deSpeckledContours_, currentPipeline.targetIntersection, currentPipeline.targetGroup); + if (groupedContours_.size() > 0) { + var finalRect = sortTargetsToOne(groupedContours_, currentPipeline.sortMode); + pipelineResult.RawPoint = finalRect; + pipelineResult.IsValid = true; + switch (currentPipeline.calibrationMode) { + case None: + ///use the center of the USBCamera to find the pitch and yaw difference + pipelineResult.CalibratedX = cameraValues.CenterX; + pipelineResult.CalibratedY = cameraValues.CenterY; + break; + case Single: + // use the static point as a calibration method instead of the center + pipelineResult.CalibratedX = currentPipeline.point.get(0).doubleValue(); + pipelineResult.CalibratedY = currentPipeline.point.get(1).doubleValue(); + break; + case Dual: + // use the calculated line to find the difference in length between the point and the line + pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.b) / currentPipeline.m; + pipelineResult.CalibratedY = (finalRect.center.x * currentPipeline.m) + currentPipeline.b; + break; + } + + pipelineResult.Pitch = cameraValues.CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY); + pipelineResult.Yaw = cameraValues.CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX); + pipelineResult.Area = finalRect.size.area(); + drawContour(outputImage, finalRect); + } + } + } + } + + return pipelineResult; + } + void hsvThreshold(Mat srcImage, Mat dst, @NotNull Scalar hsvLower, @NotNull Scalar hsvUpper, boolean shouldErode, boolean shouldDilate) { Imgproc.cvtColor(srcImage, hsvImage, Imgproc.COLOR_RGB2HSV, 3); Imgproc.blur(hsvImage, hsvImage, blur); diff --git a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java index 398274a1a..00582ea50 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java @@ -28,4 +28,7 @@ public interface CameraProcess extends Runnable { String getNickname(); void setCurrentPipelineIndex(int ntPipelineIndex); + PipelineResult runPipeline(Pipeline currentPipeline, Mat inputImage, Mat outputImage, + CameraValues cameraValues, boolean shouldFlip, boolean driverMode); + } diff --git a/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java index ce7ac6e04..eca77787b 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java @@ -38,8 +38,6 @@ public class VisionProcess implements Runnable { private Mat cameraInputMat = new Mat(); private Mat hsvThreshMat = new Mat(); private Mat streamOutputMat = new Mat(); - private Scalar contourRectColor = new Scalar(255, 0, 0); - private Scalar BoxRectColor = new Scalar(0, 0, 233); private long timeStamp = 0; public VisionProcess(CameraProcess cameraProcess) { @@ -81,19 +79,6 @@ public class VisionProcess implements Runnable { } } - private void drawContour(Mat inputMat, RotatedRect contourRect) { - if (contourRect == null) return; - List drawnContour = new ArrayList<>(); - Point[] vertices = new Point[4]; - contourRect.points(vertices); - MatOfPoint contour = new MatOfPoint(vertices); - drawnContour.add(contour); - Rect box = Imgproc.boundingRect(contour); - Imgproc.drawContours(inputMat, drawnContour, 0, contourRectColor, 3); - Imgproc.circle(inputMat, contourRect.center, 3, contourRectColor); - Imgproc.rectangle(inputMat, new Point(box.x, box.y), new Point((box.x + box.width), (box.y + box.height)), BoxRectColor, 2); - } - private void updateNetworkTables(PipelineResult pipelineResult) { if (pipelineResult.IsValid) { ntValidEntry.setBoolean(true); @@ -112,74 +97,14 @@ public class VisionProcess implements Runnable { } private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) { - var pipelineResult = new PipelineResult(); - - if (currentPipeline == null) { - return pipelineResult; - } - if (currentPipeline.orientation.equals(Orientation.Inverted)) { - Core.flip(inputImage, inputImage, -1); - } - if (cameraProcess.getDriverMode()) { - inputImage.copyTo(outputImage); - return pipelineResult; - } - Scalar hsvLower = new Scalar(currentPipeline.hue.get(0).intValue(), currentPipeline.saturation.get(0).intValue(), currentPipeline.value.get(0).intValue()); - Scalar hsvUpper = new Scalar(currentPipeline.hue.get(1).intValue(), currentPipeline.saturation.get(1).intValue(), currentPipeline.value.get(1).intValue()); - - cvProcess.hsvThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate); - - if (currentPipeline.isBinary) { - Imgproc.cvtColor(hsvThreshMat, outputImage, Imgproc.COLOR_GRAY2BGR, 3); - } else { - inputImage.copyTo(outputImage); - } - foundContours = cvProcess.findContours(hsvThreshMat); - if (foundContours.size() > 0) { - filteredContours = cvProcess.filterContours(foundContours, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent); - if (filteredContours.size() > 0) { - deSpeckledContours = cvProcess.rejectSpeckles(filteredContours, currentPipeline.speckle.doubleValue()); - if (deSpeckledContours.size() > 0) { - groupedContours = cvProcess.groupTargets(deSpeckledContours, currentPipeline.targetIntersection, currentPipeline.targetGroup); - if (groupedContours.size() > 0) { - var finalRect = cvProcess.sortTargetsToOne(groupedContours, currentPipeline.sortMode); - pipelineResult.RawPoint = finalRect; - pipelineResult.IsValid = true; - switch (currentPipeline.calibrationMode) { - case None: - ///use the center of the USBCamera to find the pitch and yaw difference - pipelineResult.CalibratedX = cameraProcess.getCamVals().CenterX; - pipelineResult.CalibratedY = cameraProcess.getCamVals().CenterY; - break; - case Single: - // use the static point as a calibration method instead of the center - pipelineResult.CalibratedX = currentPipeline.point.get(0).doubleValue(); - pipelineResult.CalibratedY = currentPipeline.point.get(1).doubleValue(); - break; - case Dual: - // use the calculated line to find the difference in length between the point and the line - pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.b) / currentPipeline.m; - pipelineResult.CalibratedY = (finalRect.center.x * currentPipeline.m) + currentPipeline.b; - break; - } -// var camVals = cameraProcess.getCamVals(); -// if (currentPipeline.isCalibrated) { -// pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.b) / currentPipeline.m; -// pipelineResult.CalibratedY = (finalRect.center.x * currentPipeline.m) + currentPipeline.b; -// } else { -// pipelineResult.CalibratedX = camVals.CenterX; -// pipelineResult.CalibratedY = camVals.CenterY; -// } - pipelineResult.Pitch = cameraProcess.getCamVals().CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY); - pipelineResult.Yaw = cameraProcess.getCamVals().CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX); - pipelineResult.Area = finalRect.size.area(); - drawContour(outputImage, finalRect); - } - } - } - } - - return pipelineResult; + return cameraProcess.runPipeline( + currentPipeline, + inputImage, + outputImage, + cameraProcess.getCamVals(), + currentPipeline.orientation.equals(Orientation.Inverted), + cameraProcess.getDriverMode() + ); } @Override