From 3d1de034c748f6548738daaec6c2e9fa0c39dcc0 Mon Sep 17 00:00:00 2001 From: Banks Troutman Date: Sat, 21 Sep 2019 13:30:09 -0400 Subject: [PATCH] Rename Processes, move camera frame get to the publishing thread --- .../main/java/com/chameleonvision/Main.java | 4 +- .../vision/process/CVProcess.java | 199 +++++++++ .../vision/process/CameraProcess.java | 253 ++---------- .../vision/process/StreamProcess.java | 42 -- .../vision/process/VisionProcess.java | 381 ++++++++++-------- Main/src/main/resources/META-INF/MANIFEST.MF | 2 + 6 files changed, 448 insertions(+), 433 deletions(-) create mode 100644 Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java delete mode 100644 Main/src/main/java/com/chameleonvision/vision/process/StreamProcess.java create mode 100644 Main/src/main/resources/META-INF/MANIFEST.MF diff --git a/Main/src/main/java/com/chameleonvision/Main.java b/Main/src/main/java/com/chameleonvision/Main.java index 638d73d0e..b0c2463ff 100644 --- a/Main/src/main/java/com/chameleonvision/Main.java +++ b/Main/src/main/java/com/chameleonvision/Main.java @@ -2,7 +2,7 @@ package com.chameleonvision; import com.chameleonvision.settings.SettingsManager; import com.chameleonvision.vision.camera.CameraManager; -import com.chameleonvision.vision.process.CameraProcess; +import com.chameleonvision.vision.process.VisionProcess; import com.chameleonvision.web.Server; public class Main { @@ -10,7 +10,7 @@ public class Main { if (CameraManager.initializeCameras()) { SettingsManager.initialize(); for (var camSet : CameraManager.getAllCamerasByName().entrySet()) { - new Thread(new CameraProcess(camSet.getValue())).start(); + new Thread(new VisionProcess(camSet.getValue())).start(); } // NetworkTableInstance.getDefault().startClientTeam(SettingsManager.GeneralSettings.team_number); Server.main(8888); diff --git a/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java new file mode 100644 index 000000000..dca8e94a0 --- /dev/null +++ b/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java @@ -0,0 +1,199 @@ +package com.chameleonvision.vision.process; + +import com.chameleonvision.vision.camera.CameraValues; +import org.apache.commons.math3.util.FastMath; +import org.jetbrains.annotations.NotNull; +import org.opencv.core.*; +import org.opencv.imgproc.*; + +import java.util.*; + +@SuppressWarnings("WeakerAccess") +public class CVProcess { + + private HashMapTargetGrouping= new HashMap<>() {{ + put("Single", 1); + put("Dual", 2); + put("Triple", 3); + put("Quadruple", 4); + put("Quintuple", 5); + }}; + + private final CameraValues CamVals; + + CVProcess(CameraValues camVals){ + CamVals = camVals; + } + + private Mat Kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5)); + + private Mat hsvImage = new Mat(); + 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 (shouldDilate){ + Imgproc.dilate(dst, dst, Kernel); + } + hsvImage.release(); + } + + private List FoundContours = new ArrayList<>(); + private Mat binaryMat = new Mat(); + List FindContours(Mat src) { + src.copyTo(binaryMat); + FoundContours.clear(); + Imgproc.findContours(binaryMat, FoundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); + binaryMat.release(); + return FoundContours; + } + + private List FilteredContours = new ArrayList<>(); + List FilterContours(List InputContours, List area, List ratio, List extent) { + for (MatOfPoint Contour : InputContours){ + try{ + var contourArea = Imgproc.contourArea(Contour); + double targetArea = FastMath.round((contourArea / CamVals.ImageArea) * 100); + if (targetArea <= area.get(0) || targetArea >= area.get(1)){ + continue; + } + var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray())); + var targetFullness = (contourArea / rect.size.area()) * 100; + if (targetFullness <= extent.get(0) || targetArea >= extent.get(1)){ + continue; + } + var aspectRatio = rect.size.width / rect.size.height; + if (aspectRatio <= ratio.get(0) || aspectRatio >= ratio.get(1)){ + continue; + } + FilteredContours.add(Contour); + } + catch (Exception ignored) { } + } + return FilteredContours; + } + + private double calcDistance(RotatedRect rect) { + return FastMath.sqrt(FastMath.pow(CamVals.CenterX - rect.center.x, 2) + FastMath.pow(CamVals.CenterY - rect.center.y, 2)); + } + + private Comparator SortByCentermostComparator = Comparator.comparingDouble(this::calcDistance); + + RotatedRect SortTargetsToOne(List inputRects, String sortMode) { + switch (sortMode) { + case "Largest": + return Collections.max(inputRects, Comparator.comparing(rect -> rect.size.area())); + case "Smallest": + return Collections.min(inputRects, Comparator.comparing(rect -> rect.size.area())); + case "Highest": + return Collections.min(inputRects, Comparator.comparing(rect -> rect.center.y)); + case "Lowest": + return Collections.max(inputRects, Comparator.comparing(rect -> rect.center.y)); + case "Leftmost": + return Collections.min(inputRects, Comparator.comparing(rect -> rect.center.x)); + case "Rightmost": + return Collections.max(inputRects, Comparator.comparing(rect -> rect.center.x)); + case "Centermost": + return Collections.min(inputRects, SortByCentermostComparator); + default: + return inputRects.get(0); // default to whatever the first contour is, but this should never happen + } + } + + private List FinalCountours = new ArrayList<>(); + List GroupTargets(List InputContours, String IntersectionPoint, String TargetGroup) { + FinalCountours.clear(); + if (!TargetGroup.equals("Single")){ + for (var i = 0; i < InputContours.size(); i++){ + List FinalContourList = new ArrayList<>(InputContours.get(i).toList()); + for (var c = 0; c < (TargetGrouping.get(TargetGroup) - 1); c++){ + try{ + MatOfPoint firstContour = InputContours.get(i + c); + MatOfPoint secondContour = InputContours.get(i + c + 1); + if (IsIntersecting(firstContour, secondContour, IntersectionPoint)){ + FinalContourList.addAll(secondContour.toList()); + } + firstContour.release(); + secondContour.release(); + MatOfPoint2f contour = new MatOfPoint2f(); + contour.fromList(FinalContourList); + if (contour.cols() != 0 && contour.rows() != 0){ + RotatedRect rect = Imgproc.minAreaRect(contour); + FinalCountours.add(rect); + } + } catch (IndexOutOfBoundsException e){ + FinalContourList.clear(); + break; + } + } + } + + } else { + for (MatOfPoint inputContour : InputContours) { + MatOfPoint2f contour = new MatOfPoint2f(); + contour.fromArray(inputContour.toArray()); + if (contour.cols() != 0 && contour.rows() != 0) { + RotatedRect rect = Imgproc.minAreaRect(contour); + FinalCountours.add(rect); + } + } + } + return FinalCountours; + } + + private Mat intersectMatA = new Mat(); + private Mat intersectMatB = new Mat(); + private boolean IsIntersecting(MatOfPoint ContourOne, MatOfPoint ContourTwo, String IntersectionPoint) { + if (IntersectionPoint.equals("None")){ + return true; + } + try { + 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); + double vxA = intersectMatA.get(0,0)[0]; + double vyA = intersectMatA.get(1,0)[0]; + double x0A = intersectMatA.get(2,0)[0]; + double y0A = intersectMatA.get(3,0)[0]; + double mA = vyA / vxA; + double vxB = intersectMatB.get(0,0)[0]; + double vyB = intersectMatB.get(1,0)[0]; + double x0B = intersectMatB.get(2,0)[0]; + double y0B = intersectMatB.get(3,0)[0]; + double mB = vyB / vxB; + double intersectionX = (mA * x0A) - y0A - (mB * x0B) + y0B / (mA - mB); + double intersectionY = (mA * (intersectionX - x0A)) + y0A; + switch (IntersectionPoint){ + case "Up" :{ + if (intersectionY < CamVals.CenterY){ + return true; + } + break; + } + case "Down": { + if (intersectionY > CamVals.CenterY){ + return true; + } + break; + } + case "Left": { + if (intersectionX < CamVals.CenterX){ + return true; + } + break; + } + case "Right": { + if (intersectionX > CamVals.CenterX){ + return true; + } + break; + } + } + return false; + } + catch (Exception e){ + return false; + } + } +} 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 2d073076a..d7c1a0bd8 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java @@ -1,240 +1,55 @@ package com.chameleonvision.vision.process; -import com.chameleonvision.settings.SettingsManager; -import com.chameleonvision.vision.Pipeline; import com.chameleonvision.vision.camera.Camera; -import com.chameleonvision.web.ServerHandler; -import edu.wpi.first.networktables.*; -import org.opencv.core.*; -import org.opencv.imgproc.Imgproc; - -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; +import org.opencv.core.CvType; +import org.opencv.core.Mat; public class CameraProcess implements Runnable { private final Camera camera; - private final String cameraName; + private final int maxFPS; + private Mat inputFrame; + private Mat outputFrame; + private long timestamp; - // NetworkTables - private NetworkTableEntry ntPipelineEntry; - private NetworkTableEntry ntDriverModeEntry; - private NetworkTableEntry ntYawEntry; - private NetworkTableEntry ntPitchEntry; - private NetworkTableEntry ntDistanceEntry; - private NetworkTableEntry ntTimeStampEntry; - private NetworkTableEntry ntValidEntry; + private final Object inputFrameLock = new Object(); + private final Object outputFrameLock = new Object(); - // chameleon specific - private Pipeline currentPipeline; - private VisionProcess visionProcess; + public CameraProcess(Camera camera) { + this.camera = camera; + maxFPS = camera.getVideoMode().fps; + var camVals = camera.getCamVals(); + inputFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3); + outputFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3); + } - // pipeline process items - private List FoundContours = new ArrayList<>(); - private List FilteredContours = new ArrayList<>(); - private List GroupedContours = new ArrayList<>(); - private Mat cameraInputMat = new Mat(); - private Mat hsvThreshMat = new Mat(); - private Mat streamOutputMat = new Mat(); - private Scalar contourRectColor = new Scalar(255, 0, 0); - private long TimeStamp = 0; - - private final StreamProcess streamProcess; - - private void DriverModeListener(EntryNotification entryNotification) { - if (entryNotification.value.getBoolean()) { - camera.setExposure(25); - camera.setBrightness(15); - } else { - Pipeline pipeline = camera.getCurrentPipeline(); - camera.setExposure(pipeline.exposure); - camera.setBrightness(pipeline.brightness); + void updateFrame(Mat inputFrame) { + synchronized (inputFrameLock) { + inputFrame.copyTo(this.inputFrame); } } - private void PipelineListener(EntryNotification entryNotification) { - var ntPipelineIndex = Integer.parseInt(entryNotification.value.getString().replace("pipeline", "")); - if (camera.getPipelines().containsKey(ntPipelineIndex)) { -// camera.setEntryNotification.value.getString()); - var pipeline = camera.getCurrentPipeline(); - - camera.setExposure(pipeline.exposure); - camera.setBrightness(pipeline.brightness); - HashMap pipeChange = new HashMap<>(); - pipeChange.put("curr_pipeline",ntPipelineIndex); - ServerHandler.broadcastMessage(pipeChange); - - } else { - ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex()); + long getLatestFrame(Mat outputFrame) { + synchronized (outputFrameLock) { + this.outputFrame.copyTo(outputFrame); + return timestamp; } } - public CameraProcess(Camera processCam) { - camera = processCam; - this.cameraName = camera.name; - - // NetworkTables - NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraName); - ntPipelineEntry = ntTable.getEntry("Pipeline"); - ntDriverModeEntry = ntTable.getEntry("Driver_Mode"); - ntPitchEntry = ntTable.getEntry("Pitch"); - ntYawEntry = ntTable.getEntry("Yaw"); - ntDistanceEntry = ntTable.getEntry("Distance"); - ntTimeStampEntry = ntTable.getEntry("TimeStamp"); - ntValidEntry = ntTable.getEntry("Valid"); - ntDriverModeEntry.addListener(this::DriverModeListener, EntryListenerFlags.kUpdate); - ntPipelineEntry.addListener(this::PipelineListener, EntryListenerFlags.kUpdate); - ntDriverModeEntry.setBoolean(false); - ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex()); - - // camera settings - visionProcess = new VisionProcess(camera.getCamVals()); - streamProcess = new StreamProcess(camera); - } - - private void drawContour(Mat inputMat, RotatedRect contourRect) { - if (contourRect == null) return; - List drawnContour = new ArrayList<>(); - Point[] vertices = new Point[4]; - contourRect.points(vertices); - drawnContour.add(new MatOfPoint(vertices)); - Imgproc.drawContours(inputMat, drawnContour, 0, contourRectColor, 3); - Imgproc.circle(inputMat, contourRect.center, 3, contourRectColor); - } - - private void updateNetworkTables(PipelineResult pipelineResult) { - ntValidEntry.setBoolean(pipelineResult.IsValid); - if (pipelineResult.IsValid){ - ntYawEntry.setNumber(pipelineResult.Yaw); - ntPitchEntry.setNumber(pipelineResult.Pitch); - } - ntTimeStampEntry.setNumber(TimeStamp); - } - - private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) { - var pipelineResult = new PipelineResult(); - - if (currentPipeline == null) { - return pipelineResult; - } - if (!currentPipeline.orientation.equals("Normal")){ - Core.flip(inputImage,inputImage,-1); - } - if (ntDriverModeEntry.getBoolean(false)){ - inputImage.copyTo(outputImage); - return pipelineResult; - } - 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(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate); - - if (currentPipeline.is_binary == 1) { - Imgproc.cvtColor(hsvThreshMat, outputImage, Imgproc.COLOR_GRAY2BGR, 3); - } else { - inputImage.copyTo(outputImage); - } - FoundContours = visionProcess.FindContours(hsvThreshMat); - if (FoundContours.size() > 0) { - FilteredContours = visionProcess.FilterContours(FoundContours, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent); - if (FilteredContours.size() > 0) { - GroupedContours = visionProcess.GroupTargets(FilteredContours, currentPipeline.target_intersection, currentPipeline.target_group); - if (GroupedContours.size() > 0) { - var finalRect = visionProcess.SortTargetsToOne(GroupedContours, currentPipeline.sort_mode); - pipelineResult.RawPoint = finalRect; - pipelineResult.IsValid = true; - if (!currentPipeline.is_calibrated) { - pipelineResult.CalibratedX = camera.getCamVals().CenterX; - pipelineResult.CalibratedY = camera.getCamVals().CenterY; - } else { - pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.B) / currentPipeline.M; - pipelineResult.CalibratedY = finalRect.center.x * currentPipeline.M + currentPipeline.B; - } - pipelineResult.Pitch = camera.getCamVals().CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY); - pipelineResult.Yaw = camera.getCamVals().CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX); - drawContour(outputImage, finalRect); - } - } - } - - return pipelineResult; - } - @Override public void run() { - // processing time tracking - long startTime; - long fpsLastTime = 0; - double processTimeMs; - double fps = 0; - double uiFps = 0; - int maxFps = camera.getVideoMode().fps; - - new Thread(streamProcess).start(); - - long lastFrameEndNanosec = 0; - - while (!Thread.interrupted()) { - startTime = System.nanoTime(); - if ((startTime - lastFrameEndNanosec) * 1e-6 >= 1000.0/maxFps + 3) { // 3 additional fps to allow for overhead - FoundContours.clear(); - FilteredContours.clear(); - GroupedContours.clear(); - - // update FPS for ui only every 0.5 seconds - if ((startTime - fpsLastTime) * 1e-6 >= 500) { - if (fps >= maxFps) { - uiFps = maxFps; - } else { - uiFps = fps; - } - fpsLastTime = System.nanoTime(); - } - - currentPipeline = camera.getCurrentPipeline(); - // start fps counter right before grabbing input frame - TimeStamp = camera.grabFrame(cameraInputMat); - if (cameraInputMat.cols() == 0 && cameraInputMat.rows() == 0) { - continue; - } - - // get vision data - var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat); - updateNetworkTables(pipelineResult); - if (cameraName.equals(SettingsManager.GeneralSettings.curr_camera)) { - HashMap WebSend = new HashMap<>(); - HashMap point = new HashMap<>(); - List center = new ArrayList<>(); - if (pipelineResult.IsValid) { - center.add(pipelineResult.RawPoint.center.x); - center.add(pipelineResult.RawPoint.center.y); - point.put("pitch", pipelineResult.Pitch); - point.put("yaw", pipelineResult.Yaw); - } else { - center.add(0.0); - center.add(0.0); - point.put("pitch", 0); - point.put("yaw", 0); - } - point.put("fps", uiFps); - WebSend.put("point", point); - WebSend.put("raw_point", center); - ServerHandler.broadcastMessage(WebSend); - } - - //camera.putFrame(streamOutputMat); - streamProcess.updateFrame(streamOutputMat); - - cameraInputMat.release(); - hsvThreshMat.release(); - - // calculate FPS - lastFrameEndNanosec = System.nanoTime(); - processTimeMs = (lastFrameEndNanosec - startTime) * 1e-6; - fps = 1000 / processTimeMs; - - System.out.printf("%s - Process time: %-5.2fms, FPS: %-5.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n", cameraName, processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size()); + while(!Thread.interrupted()) { + synchronized (outputFrameLock) { + timestamp = camera.grabFrame(outputFrame); + } + synchronized (inputFrameLock) { + camera.putFrame(inputFrame); + } + var msToWait = (long)1000/maxFPS; + try { + Thread.sleep(msToWait); + } catch (InterruptedException e) { + e.printStackTrace(); } } } diff --git a/Main/src/main/java/com/chameleonvision/vision/process/StreamProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/StreamProcess.java deleted file mode 100644 index ac3d7e951..000000000 --- a/Main/src/main/java/com/chameleonvision/vision/process/StreamProcess.java +++ /dev/null @@ -1,42 +0,0 @@ -package com.chameleonvision.vision.process; - -import com.chameleonvision.vision.camera.Camera; -import org.opencv.core.CvType; -import org.opencv.core.Mat; - -public class StreamProcess implements Runnable { - - private final Camera camera; - private final int maxFPS; - private Mat streamFrame; - - private final Object streamFrameLock = new Object(); - - public StreamProcess(Camera camera) { - this.camera = camera; - maxFPS = camera.getVideoMode().fps; - var camVals = camera.getCamVals(); - streamFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3); - } - - void updateFrame(Mat inputFrame) { - synchronized (streamFrameLock) { - inputFrame.copyTo(streamFrame); - } - } - - @Override - public void run() { - while(!Thread.interrupted()) { - synchronized (streamFrameLock) { - camera.putFrame(streamFrame); - } - var msToWait = (long)1000/maxFPS; - try { - Thread.sleep(msToWait); - } catch (InterruptedException e) { - e.printStackTrace(); - } - } - } -} 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 6d7b2a1ec..304bbbfef 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java @@ -1,199 +1,240 @@ package com.chameleonvision.vision.process; -import com.chameleonvision.vision.camera.CameraValues; -import org.apache.commons.math3.util.FastMath; -import org.jetbrains.annotations.NotNull; +import com.chameleonvision.settings.SettingsManager; +import com.chameleonvision.vision.Pipeline; +import com.chameleonvision.vision.camera.Camera; +import com.chameleonvision.web.ServerHandler; +import edu.wpi.first.networktables.*; import org.opencv.core.*; -import org.opencv.imgproc.*; +import org.opencv.imgproc.Imgproc; -import java.util.*; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; -@SuppressWarnings("WeakerAccess") -public class VisionProcess { +public class VisionProcess implements Runnable { - private HashMapTargetGrouping= new HashMap<>() {{ - put("Single", 1); - put("Dual", 2); - put("Triple", 3); - put("Quadruple", 4); - put("Quintuple", 5); - }}; + private final Camera camera; + private final String cameraName; - private final CameraValues CamVals; + // NetworkTables + private NetworkTableEntry ntPipelineEntry; + private NetworkTableEntry ntDriverModeEntry; + private NetworkTableEntry ntYawEntry; + private NetworkTableEntry ntPitchEntry; + private NetworkTableEntry ntDistanceEntry; + private NetworkTableEntry ntTimeStampEntry; + private NetworkTableEntry ntValidEntry; - VisionProcess(CameraValues camVals){ - CamVals = camVals; - } - - private Mat Kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5)); - - private Mat hsvImage = new Mat(); - 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 (shouldDilate){ - Imgproc.dilate(dst, dst, Kernel); - } - hsvImage.release(); - } + // chameleon specific + private Pipeline currentPipeline; + private CVProcess cvProcess; + // pipeline process items private List FoundContours = new ArrayList<>(); - private Mat binaryMat = new Mat(); - List FindContours(Mat src) { - src.copyTo(binaryMat); - FoundContours.clear(); - Imgproc.findContours(binaryMat, FoundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); - binaryMat.release(); - return FoundContours; - } - private List FilteredContours = new ArrayList<>(); - List FilterContours(List InputContours, List area, List ratio, List extent) { - for (MatOfPoint Contour : InputContours){ - try{ - var contourArea = Imgproc.contourArea(Contour); - double targetArea = FastMath.round((contourArea / CamVals.ImageArea) * 100); - if (targetArea <= area.get(0) || targetArea >= area.get(1)){ - continue; - } - var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray())); - var targetFullness = (contourArea / rect.size.area()) * 100; - if (targetFullness <= extent.get(0) || targetArea >= extent.get(1)){ - continue; - } - var aspectRatio = rect.size.width / rect.size.height; - if (aspectRatio <= ratio.get(0) || aspectRatio >= ratio.get(1)){ - continue; - } - FilteredContours.add(Contour); - } - catch (Exception ignored) { } - } - return FilteredContours; - } + private List GroupedContours = new ArrayList<>(); + private Mat cameraInputMat = new Mat(); + private Mat hsvThreshMat = new Mat(); + private Mat streamOutputMat = new Mat(); + private Scalar contourRectColor = new Scalar(255, 0, 0); + private long TimeStamp = 0; - private double calcDistance(RotatedRect rect) { - return FastMath.sqrt(FastMath.pow(CamVals.CenterX - rect.center.x, 2) + FastMath.pow(CamVals.CenterY - rect.center.y, 2)); - } + private final CameraProcess cameraProcess; - private Comparator SortByCentermostComparator = Comparator.comparingDouble(this::calcDistance); - - RotatedRect SortTargetsToOne(List inputRects, String sortMode) { - switch (sortMode) { - case "Largest": - return Collections.max(inputRects, Comparator.comparing(rect -> rect.size.area())); - case "Smallest": - return Collections.min(inputRects, Comparator.comparing(rect -> rect.size.area())); - case "Highest": - return Collections.min(inputRects, Comparator.comparing(rect -> rect.center.y)); - case "Lowest": - return Collections.max(inputRects, Comparator.comparing(rect -> rect.center.y)); - case "Leftmost": - return Collections.min(inputRects, Comparator.comparing(rect -> rect.center.x)); - case "Rightmost": - return Collections.max(inputRects, Comparator.comparing(rect -> rect.center.x)); - case "Centermost": - return Collections.min(inputRects, SortByCentermostComparator); - default: - return inputRects.get(0); // default to whatever the first contour is, but this should never happen + private void DriverModeListener(EntryNotification entryNotification) { + if (entryNotification.value.getBoolean()) { + camera.setExposure(25); + camera.setBrightness(15); + } else { + Pipeline pipeline = camera.getCurrentPipeline(); + camera.setExposure(pipeline.exposure); + camera.setBrightness(pipeline.brightness); } } - private List FinalCountours = new ArrayList<>(); - List GroupTargets(List InputContours, String IntersectionPoint, String TargetGroup) { - FinalCountours.clear(); - if (!TargetGroup.equals("Single")){ - for (var i = 0; i < InputContours.size(); i++){ - List FinalContourList = new ArrayList<>(InputContours.get(i).toList()); - for (var c = 0; c < (TargetGrouping.get(TargetGroup) - 1); c++){ - try{ - MatOfPoint firstContour = InputContours.get(i + c); - MatOfPoint secondContour = InputContours.get(i + c + 1); - if (IsIntersecting(firstContour, secondContour, IntersectionPoint)){ - FinalContourList.addAll(secondContour.toList()); - } - firstContour.release(); - secondContour.release(); - MatOfPoint2f contour = new MatOfPoint2f(); - contour.fromList(FinalContourList); - if (contour.cols() != 0 && contour.rows() != 0){ - RotatedRect rect = Imgproc.minAreaRect(contour); - FinalCountours.add(rect); - } - } catch (IndexOutOfBoundsException e){ - FinalContourList.clear(); - break; - } - } - } + private void PipelineListener(EntryNotification entryNotification) { + var ntPipelineIndex = Integer.parseInt(entryNotification.value.getString().replace("pipeline", "")); + if (camera.getPipelines().containsKey(ntPipelineIndex)) { +// camera.setEntryNotification.value.getString()); + var pipeline = camera.getCurrentPipeline(); + + camera.setExposure(pipeline.exposure); + camera.setBrightness(pipeline.brightness); + HashMap pipeChange = new HashMap<>(); + pipeChange.put("curr_pipeline",ntPipelineIndex); + ServerHandler.broadcastMessage(pipeChange); } else { - for (MatOfPoint inputContour : InputContours) { - MatOfPoint2f contour = new MatOfPoint2f(); - contour.fromArray(inputContour.toArray()); - if (contour.cols() != 0 && contour.rows() != 0) { - RotatedRect rect = Imgproc.minAreaRect(contour); - FinalCountours.add(rect); - } - } + ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex()); } - return FinalCountours; } - private Mat intersectMatA = new Mat(); - private Mat intersectMatB = new Mat(); - private boolean IsIntersecting(MatOfPoint ContourOne, MatOfPoint ContourTwo, String IntersectionPoint) { - if (IntersectionPoint.equals("None")){ - return true; + public VisionProcess(Camera processCam) { + camera = processCam; + this.cameraName = camera.name; + + // NetworkTables + NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraName); + ntPipelineEntry = ntTable.getEntry("Pipeline"); + ntDriverModeEntry = ntTable.getEntry("Driver_Mode"); + ntPitchEntry = ntTable.getEntry("Pitch"); + ntYawEntry = ntTable.getEntry("Yaw"); + ntDistanceEntry = ntTable.getEntry("Distance"); + ntTimeStampEntry = ntTable.getEntry("TimeStamp"); + ntValidEntry = ntTable.getEntry("Valid"); + ntDriverModeEntry.addListener(this::DriverModeListener, EntryListenerFlags.kUpdate); + ntPipelineEntry.addListener(this::PipelineListener, EntryListenerFlags.kUpdate); + ntDriverModeEntry.setBoolean(false); + ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex()); + + // camera settings + cvProcess = new CVProcess(camera.getCamVals()); + cameraProcess = new CameraProcess(camera); + } + + private void drawContour(Mat inputMat, RotatedRect contourRect) { + if (contourRect == null) return; + List drawnContour = new ArrayList<>(); + Point[] vertices = new Point[4]; + contourRect.points(vertices); + drawnContour.add(new MatOfPoint(vertices)); + Imgproc.drawContours(inputMat, drawnContour, 0, contourRectColor, 3); + Imgproc.circle(inputMat, contourRect.center, 3, contourRectColor); + } + + private void updateNetworkTables(PipelineResult pipelineResult) { + ntValidEntry.setBoolean(pipelineResult.IsValid); + if (pipelineResult.IsValid){ + ntYawEntry.setNumber(pipelineResult.Yaw); + ntPitchEntry.setNumber(pipelineResult.Pitch); } - try { - 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); - double vxA = intersectMatA.get(0,0)[0]; - double vyA = intersectMatA.get(1,0)[0]; - double x0A = intersectMatA.get(2,0)[0]; - double y0A = intersectMatA.get(3,0)[0]; - double mA = vyA / vxA; - double vxB = intersectMatB.get(0,0)[0]; - double vyB = intersectMatB.get(1,0)[0]; - double x0B = intersectMatB.get(2,0)[0]; - double y0B = intersectMatB.get(3,0)[0]; - double mB = vyB / vxB; - double intersectionX = (mA * x0A) - y0A - (mB * x0B) + y0B / (mA - mB); - double intersectionY = (mA * (intersectionX - x0A)) + y0A; - switch (IntersectionPoint){ - case "Up" :{ - if (intersectionY < CamVals.CenterY){ - return true; + ntTimeStampEntry.setNumber(TimeStamp); + } + + private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) { + var pipelineResult = new PipelineResult(); + + if (currentPipeline == null) { + return pipelineResult; + } + if (!currentPipeline.orientation.equals("Normal")){ + Core.flip(inputImage,inputImage,-1); + } + if (ntDriverModeEntry.getBoolean(false)){ + inputImage.copyTo(outputImage); + return pipelineResult; + } + 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)); + + cvProcess.HSVThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate); + + if (currentPipeline.is_binary == 1) { + 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) { + GroupedContours = cvProcess.GroupTargets(FilteredContours, currentPipeline.target_intersection, currentPipeline.target_group); + if (GroupedContours.size() > 0) { + var finalRect = cvProcess.SortTargetsToOne(GroupedContours, currentPipeline.sort_mode); + pipelineResult.RawPoint = finalRect; + pipelineResult.IsValid = true; + if (!currentPipeline.is_calibrated) { + pipelineResult.CalibratedX = camera.getCamVals().CenterX; + pipelineResult.CalibratedY = camera.getCamVals().CenterY; + } else { + pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.B) / currentPipeline.M; + pipelineResult.CalibratedY = finalRect.center.x * currentPipeline.M + currentPipeline.B; } - break; - } - case "Down": { - if (intersectionY > CamVals.CenterY){ - return true; - } - break; - } - case "Left": { - if (intersectionX < CamVals.CenterX){ - return true; - } - break; - } - case "Right": { - if (intersectionX > CamVals.CenterX){ - return true; - } - break; + pipelineResult.Pitch = camera.getCamVals().CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY); + pipelineResult.Yaw = camera.getCamVals().CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX); + drawContour(outputImage, finalRect); } } - return false; } - catch (Exception e){ - return false; + + return pipelineResult; + } + + @Override + public void run() { + // processing time tracking + long startTime; + long fpsLastTime = 0; + double processTimeMs; + double fps = 0; + double uiFps = 0; + int maxFps = camera.getVideoMode().fps; + + new Thread(cameraProcess).start(); + + long lastFrameEndNanosec = 0; + + while (!Thread.interrupted()) { + startTime = System.nanoTime(); + if ((startTime - lastFrameEndNanosec) * 1e-6 >= 1000.0/maxFps + 3) { // 3 additional fps to allow for overhead + FoundContours.clear(); + FilteredContours.clear(); + GroupedContours.clear(); + + // update FPS for ui only every 0.5 seconds + if ((startTime - fpsLastTime) * 1e-6 >= 500) { + if (fps >= maxFps) { + uiFps = maxFps; + } else { + uiFps = fps; + } + fpsLastTime = System.nanoTime(); + } + + currentPipeline = camera.getCurrentPipeline(); + // start fps counter right before grabbing input frame + TimeStamp = cameraProcess.getLatestFrame(cameraInputMat); + if (cameraInputMat.cols() == 0 && cameraInputMat.rows() == 0) { + continue; + } + + // get vision data + var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat); + updateNetworkTables(pipelineResult); + if (cameraName.equals(SettingsManager.GeneralSettings.curr_camera)) { + HashMap WebSend = new HashMap<>(); + HashMap point = new HashMap<>(); + List center = new ArrayList<>(); + if (pipelineResult.IsValid) { + center.add(pipelineResult.RawPoint.center.x); + center.add(pipelineResult.RawPoint.center.y); + point.put("pitch", pipelineResult.Pitch); + point.put("yaw", pipelineResult.Yaw); + } else { + center.add(0.0); + center.add(0.0); + point.put("pitch", 0); + point.put("yaw", 0); + } + point.put("fps", uiFps); + WebSend.put("point", point); + WebSend.put("raw_point", center); + ServerHandler.broadcastMessage(WebSend); + } + + cameraProcess.updateFrame(streamOutputMat); + + cameraInputMat.release(); + hsvThreshMat.release(); + + // calculate FPS + lastFrameEndNanosec = System.nanoTime(); + processTimeMs = (lastFrameEndNanosec - startTime) * 1e-6; + fps = 1000 / processTimeMs; + + System.out.printf("%s - Process time: %-5.2fms, FPS: %-5.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n", cameraName, processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size()); + } } } } diff --git a/Main/src/main/resources/META-INF/MANIFEST.MF b/Main/src/main/resources/META-INF/MANIFEST.MF new file mode 100644 index 000000000..d1b11537d --- /dev/null +++ b/Main/src/main/resources/META-INF/MANIFEST.MF @@ -0,0 +1,2 @@ +Manifest-Version: 1.0 +Main-Class: com.chameleonvision.Main