From 86b811a36d10fb8c9f6dfd3dbcc901a5377075b8 Mon Sep 17 00:00:00 2001 From: Banks Troutman Date: Mon, 16 Sep 2019 04:08:23 -0400 Subject: [PATCH] Vision work, move camera calcs to CameraValues class --- .../com/chameleonvision/MemoryManager.java | 8 +- .../chameleonvision/vision/CameraValues.java | 38 ++++++ .../com/chameleonvision/vision/Pipeline.java | 8 +- .../vision/process/CameraProcess.java | 124 ++++++++++++------ .../vision/process/VisionProcess.java | 51 ++++--- 5 files changed, 153 insertions(+), 76 deletions(-) create mode 100644 Main/src/main/java/com/chameleonvision/vision/CameraValues.java diff --git a/Main/src/main/java/com/chameleonvision/MemoryManager.java b/Main/src/main/java/com/chameleonvision/MemoryManager.java index 881789dfd..d1b34d61b 100644 --- a/Main/src/main/java/com/chameleonvision/MemoryManager.java +++ b/Main/src/main/java/com/chameleonvision/MemoryManager.java @@ -25,17 +25,19 @@ public class MemoryManager { System.runFinalization(); } - public void run() { + public void run() { run(false); } + + public void run(boolean print) { var usedMem = getUsedMemoryMB(); if (usedMem != lastUsedMb) { lastUsedMb = usedMem; - System.out.printf("Memory usage: %dMB\n", usedMem); + if (print) System.out.printf("Memory usage: %dMB\n", usedMem); } if (usedMem >= collectionThreshold) { collect(); - System.out.printf("Garbage collected at %dMB\n", usedMem); + if (print) System.out.printf("Garbage collected at %dMB\n", usedMem); } } } diff --git a/Main/src/main/java/com/chameleonvision/vision/CameraValues.java b/Main/src/main/java/com/chameleonvision/vision/CameraValues.java new file mode 100644 index 000000000..3351db7d3 --- /dev/null +++ b/Main/src/main/java/com/chameleonvision/vision/CameraValues.java @@ -0,0 +1,38 @@ +package com.chameleonvision.vision; + +import org.apache.commons.math3.fraction.Fraction; +import org.apache.commons.math3.util.FastMath; + +public class CameraValues { + public final int ImageWidth; + public final int ImageHeight; + public final double FOV; + public final double ImageArea; + public final double CenterX; + public final double CenterY; + public final double DiagonalView; + public final Fraction AspectFraction; + public final int HorizontalRatio; + public final int VerticalRatio; + public final double HorizontalView; + public final double VerticalView; + public final double HorizontalFocalLength; + public final double VerticalFocalLength; + + public CameraValues(int imageWidth, int imageHeight, double fov) { + ImageWidth = imageWidth; + ImageHeight = imageHeight; + FOV = fov; + ImageArea = ImageWidth * ImageHeight; + CenterX = ((double) ImageWidth / 2) - 0.5; + CenterY = ((double) ImageHeight / 2) - 0.5; + DiagonalView = FastMath.toRadians(FOV); + AspectFraction = new Fraction(ImageWidth, ImageHeight); + HorizontalRatio = AspectFraction.getNumerator(); + VerticalRatio = AspectFraction.getDenominator(); + HorizontalView = FastMath.atan(FastMath.tan(DiagonalView / 2) * (HorizontalRatio / DiagonalView)) * 2; + VerticalView = FastMath.atan(FastMath.tan(DiagonalView/2) * (VerticalRatio / DiagonalView)) * 2; + HorizontalFocalLength = ImageWidth / (2 * FastMath.tan(HorizontalView /2)); + VerticalFocalLength = ImageWidth / (2 * FastMath.tan(VerticalView /2)); + } +} diff --git a/Main/src/main/java/com/chameleonvision/vision/Pipeline.java b/Main/src/main/java/com/chameleonvision/vision/Pipeline.java index ef63c57cf..bd9407a13 100644 --- a/Main/src/main/java/com/chameleonvision/vision/Pipeline.java +++ b/Main/src/main/java/com/chameleonvision/vision/Pipeline.java @@ -6,15 +6,15 @@ public class Pipeline { public int exposure = 50; public int brightness = 50; public String orientation = "Normal"; - public List hue = Arrays.asList(0,100); - public List saturation = Arrays.asList(0,100); - public List value = Arrays.asList(0,100); + public List hue = Arrays.asList(50,180); + public List saturation = Arrays.asList(50,255); + public List value = Arrays.asList(50,255); public boolean erode = false; public boolean dilate = false; public List area = Arrays.asList(0,100); public List ratio = Arrays.asList(0,20); public List extent = Arrays.asList(0,100); - public boolean is_binary = false; + public int is_binary = 0; public String sort_mode = "Largest"; public String target_group = "Single"; public String target_intersection = "Largest"; 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 66e5ffabd..84c75c8c2 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java @@ -1,63 +1,105 @@ package com.chameleonvision.vision.process; +import com.chameleonvision.MemoryManager; import com.chameleonvision.settings.SettingsManager; +import com.chameleonvision.vision.CameraValues; import com.chameleonvision.vision.Pipeline; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.cameraserver.CameraServer; -import org.opencv.core.Mat; -import org.apache.commons.math3.fraction.Fraction; -import org.apache.commons.math3.util.FastMath; +import org.opencv.core.*; +import org.opencv.imgproc.Imgproc; -public class CameraProcess implements Runnable{ +import java.util.ArrayList; +import java.util.List; + +public class CameraProcess implements Runnable { private String CameraName; + + private CameraServer cs = CameraServer.getInstance(); + private NetworkTableEntry ntPipelineEntry, ntDriverModeEntry; + + private MemoryManager memManager = new MemoryManager(125); + + private int imgWidth, imgHeight; + + public CameraProcess(String CameraName){ this.CameraName = CameraName; + + // add pipeline + SettingsManager.CamerasCurrentPipeline.put(CameraName, SettingsManager.Cameras.get(CameraName).pipelines.keySet().toArray()[0].toString()); + + // NetworkTables + NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + CameraName); + ntPipelineEntry = ntTable.getEntry("Pipeline"); + ntDriverModeEntry = ntTable.getEntry("Driver_Mode"); + + imgWidth = SettingsManager.Cameras.get(CameraName).camVideoMode.width; + imgHeight = SettingsManager.Cameras.get(CameraName).camVideoMode.height; } + + @Override public void run() { + var cv_sink = cs.getVideo(SettingsManager.UsbCameras.get(CameraName)); + var cv_publish = cs.putVideo(CameraName, imgWidth, imgHeight); + double fov = SettingsManager.Cameras.get(CameraName).FOV; + CameraValues camVals = new CameraValues(imgWidth, imgHeight, fov); + VisionProcess visionProcess = new VisionProcess(camVals); + Pipeline currentPipeline; - //calling all classes - CameraServer cs = CameraServer.getInstance(); - NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); - SettingsManager manager = SettingsManager.getInstance(); - SettingsManager.CamerasCurrentPipeline.put(CameraName, SettingsManager.Cameras.get(CameraName).pipelines.keySet().toArray()[0].toString()); - //Setting up camera and network table - var Table = networkTableInstance.getTable("/Chameleon-vision/" + CameraName); - var PipeLineEntry = Table.getEntry("Pipeline"); - var DriverModeEntry = Table.getEntry("Driver_Mode"); - var cv_sink = cs.getVideo(manager.UsbCameras.get(CameraName)); + List FoundContours = new ArrayList<>(); + List FilteredContours = new ArrayList<>(); + Mat inputMat = new Mat(); + Mat bgrMat = new Mat(); + Mat hsvThreshMat = new Mat(); + Mat outputMat = new Mat(); + Mat contourBoxPointsMat = new Mat(); + Scalar contourColor = new Scalar(255, 0, 0); + long startTime, endTime; + + while (!Thread.interrupted()) { + startTime = System.nanoTime(); + FoundContours.clear(); + FilteredContours.clear(); - int Width = manager.Cameras.get(CameraName).camVideoMode.width; - int Height = manager.Cameras.get(CameraName).camVideoMode.height; - var cv_publish = cs.putVideo(CameraName,Width,Height); - //initial math setup for camera - double DiagonalView = FastMath.toRadians(manager.Cameras.get(CameraName).FOV); - Fraction AspectFraction = new Fraction(Width,Height); - int HorizontalRatio = AspectFraction.getNumerator(); - int VerticalRatio = AspectFraction.getDenominator(); - double HorizontalView = FastMath.atan(FastMath.tan(DiagonalView/2) * (HorizontalRatio / DiagonalView)) * 2; - double VerticalView = FastMath.atan(FastMath.tan(DiagonalView/2) * (VerticalRatio / DiagonalView)) * 2; - double H_FOCAL_LENGTH = Width / (2 * FastMath.tan(HorizontalView /2)); - double V_FOCAL_LENGTH = Width / (2 * FastMath.tan(VerticalView /2)); - double CenterX = ((double) Width / 2) - 0.5; - double CenterY = ((double) Height/2) - 0.5; - double CamArea = (double)(Width * Height); - VisionProcess visionProcess = new VisionProcess(CenterX,CenterY,CamArea); - Mat mat = new Mat(); - long time; + currentPipeline = SettingsManager.Cameras.get(CameraName).pipelines.get(SettingsManager.CamerasCurrentPipeline.get(CameraName)); + cv_sink.grabFrame(inputMat); + if (inputMat.cols() !=0 && inputMat.rows() != 0) { + Imgproc.cvtColor(inputMat, bgrMat, Imgproc.COLOR_RGB2BGR, 3); - while (!Thread.interrupted()){ - Pipeline pipeline = manager.Cameras.get(CameraName).pipelines.get(manager.CamerasCurrentPipeline.get(CameraName)); - time = cv_sink.grabFrame(mat); - if (mat.cols() !=0 && mat.rows() != 0) { - Mat HSVImage = visionProcess.HSVThreshold(pipeline.hue, pipeline.saturation, pipeline.value, mat, pipeline.erode, pipeline.dilate); -// List Contours = visionProcess.FindContours(HSVImage); -// List FilterdContours = visionProcess.FilterContours(Contours, pipeline.area, pipeline.ratio, pipeline.extent, pipeline.sort_mode, pipeline.target_intersection, pipeline.target_group); - cv_publish.putFrame(mat); + Scalar hsvLower = new Scalar(currentPipeline.hue.get(0), currentPipeline.saturation.get(0), currentPipeline.value.get(0)); + Scalar hsvUpper = new Scalar(currentPipeline.hue.get(1), currentPipeline.saturation.get(1), currentPipeline.value.get(1)); + + visionProcess.HSVThreshold(inputMat, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate); + FoundContours = visionProcess.FindContours(hsvThreshMat); + FilteredContours = visionProcess.FilterContours(FoundContours, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent, currentPipeline.sort_mode, currentPipeline.target_intersection, currentPipeline.target_group); + + if (currentPipeline.is_binary == 1) { + Imgproc.cvtColor(hsvThreshMat, hsvThreshMat, Imgproc.COLOR_GRAY2BGR, 3); + outputMat = hsvThreshMat; + } else { + outputMat = inputMat; + } + + if (FilteredContours.size() > 0) { + for (int i = 0; i < FilteredContours.size(); i++) { + Imgproc.drawContours(outputMat, FilteredContours, i, contourColor, 3); + } + } + + cv_publish.putFrame(outputMat); + inputMat.release(); + hsvThreshMat.release(); + for (MatOfPoint oldMat : FoundContours) { oldMat.release(); } + for (MatOfPoint oldMat1 : FilteredContours) { oldMat1.release(); } } - + memManager.run(); + endTime = System.nanoTime(); } } 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 79e41c727..f51c824bd 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java @@ -1,5 +1,6 @@ package com.chameleonvision.vision.process; +import com.chameleonvision.vision.CameraValues; import org.jetbrains.annotations.NotNull; import org.opencv.core.*; import org.opencv.imgproc.*; @@ -18,48 +19,44 @@ public class VisionProcess { put("Quintuple", 5); }}; - private double CamArea, CenterX, CenterY; + private final CameraValues CamVals; - VisionProcess(double CenterX, double CenterY, double CamArea){ - this.CenterX = CenterX; - this.CenterY = CenterY; - this.CamArea = CamArea; + VisionProcess(CameraValues camVals){ + CamVals = camVals; } private Mat Kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5)); - private Mat hsvMat = new Mat(); - private Mat hsvThreshMat = new Mat(); - private Scalar hsvLower, hsvUpper; + private Mat hsvImage = new Mat(); - Mat HSVThreshold(@NotNull List hue, @NotNull List saturation, @NotNull List value, Mat image, boolean IsErode, boolean IsDilate){ - Imgproc.cvtColor(image, hsvMat,Imgproc.COLOR_BGR2HSV,3); - hsvLower = new Scalar(hue.get(0), saturation.get(0), value.get(0)); - hsvUpper = new Scalar(hue.get(1), saturation.get(1), value.get(1)); - Core.inRange(hsvMat, hsvLower, hsvUpper, hsvThreshMat); - if (IsErode){ - Imgproc.erode(hsvThreshMat, hsvThreshMat, Kernel); + void HSVThreshold(Mat srcImage, Mat dst, @NotNull Scalar hsvLower, @NotNull Scalar hsvUpper, boolean shouldErode, boolean shouldDilate) { + Imgproc.cvtColor(srcImage, hsvImage, Imgproc.COLOR_RGB2HSV,3); + Core.inRange(hsvImage, hsvLower, hsvUpper, dst); + if (shouldErode){ + Imgproc.erode(dst, dst, Kernel); } - if (IsDilate){ - Imgproc.dilate(hsvThreshMat, hsvThreshMat, Kernel); + if (shouldDilate){ + Imgproc.dilate(dst, dst, Kernel); } - return hsvThreshMat; + hsvImage.release(); } private List FoundContours = new ArrayList<>(); - public List FindContours(Mat BinaryImage){ + private Mat binaryMat = new Mat(); + List FindContours(Mat src) { + src.copyTo(binaryMat); FoundContours.clear(); - Imgproc.findContours(BinaryImage, FoundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); - BinaryImage.release(); + Imgproc.findContours(binaryMat, FoundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); + binaryMat.release(); return FoundContours; } private List FilteredContours = new ArrayList(); - public List FilterContours(List InputContours, List area, List ratio, List extent, String SortMode, String TargetIntersection, String TargetGrouping){ + List FilterContours(List InputContours, List area, List ratio, List extent, String SortMode, String TargetIntersection, String TargetGrouping){ for (MatOfPoint Contour : InputContours){ try{ var contourArea = Imgproc.contourArea(Contour); - double targetArea = (contourArea / CamArea) * 100; + double targetArea = (contourArea / CamVals.ImageArea) * 100; if (targetArea >= area.get(0) || targetArea <= area.get(1)){ continue; } @@ -74,15 +71,13 @@ public class VisionProcess { } FilteredContours.add(Contour); } - catch (Exception e) { - - } + catch (Exception ignored) { } } return FilteredContours; } private List FinalCountours = new ArrayList<>(); - private List GroupTargets(List InputContours, String IntersectionPoint,String TargetGroup) { + private List GroupTargets(List InputContours, String IntersectionPoint, String TargetGroup) { FinalCountours.clear(); if (!TargetGroup.equals("Single")){ for (var i = 0; i < InputContours.size(); i++){ @@ -112,7 +107,7 @@ public class VisionProcess { private boolean IsIntersecting(MatOfPoint ContourOne, MatOfPoint ContourTwo, String IntersectionPoint) { Imgproc.fitLine(ContourOne, intersectMatA, Imgproc.CV_DIST_L2,0,0.01,0.01); Imgproc.fitLine(ContourTwo, intersectMatB, Imgproc.CV_DIST_L2,0,0.01,0.01); -// Rect2d = + return true; } }