diff --git a/Main/src/main/java/com/chameleonvision/vision/CameraValues.java b/Main/src/main/java/com/chameleonvision/vision/CameraValues.java index 5e7bdfbb4..e6def334c 100644 --- a/Main/src/main/java/com/chameleonvision/vision/CameraValues.java +++ b/Main/src/main/java/com/chameleonvision/vision/CameraValues.java @@ -19,6 +19,10 @@ public class CameraValues { public final double HorizontalFocalLength; public final double VerticalFocalLength; + public CameraValues(Camera camera) { + this(camera.camVideoMode.width, camera.camVideoMode.height, camera.FOV); + } + public CameraValues(int imageWidth, int imageHeight, double fov) { ImageWidth = imageWidth; ImageHeight = imageHeight; 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 e4b358726..aa4bcf62a 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java @@ -4,31 +4,51 @@ import com.chameleonvision.MemoryManager; import com.chameleonvision.settings.SettingsManager; import com.chameleonvision.vision.CameraValues; import com.chameleonvision.vision.Pipeline; -import com.chameleonvision.web.Server; +import edu.wpi.cscore.CvSink; +import edu.wpi.cscore.CvSource; import edu.wpi.first.networktables.*; import edu.wpi.first.cameraserver.CameraServer; -import org.apache.commons.math3.stat.descriptive.moment.Mean; -import org.apache.commons.math3.stat.descriptive.moment.Variance; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; import java.util.ArrayList; import java.util.List; -import java.util.Set; public class CameraProcess implements Runnable { private String CameraName; - private CameraServer cs = CameraServer.getInstance(); - private NetworkTableEntry ntPipelineEntry, ntDriverModeEntry, ntYawEntry, ntPitchEntry, ntDistanceEntry, ntTimeStampEntry, ntValidEntry; + // NetworkTables + private NetworkTableEntry ntPipelineEntry; + private NetworkTableEntry ntDriverModeEntry; + private NetworkTableEntry ntYawEntry; + private NetworkTableEntry ntPitchEntry; + private NetworkTableEntry ntDistanceEntry; + private NetworkTableEntry ntTimeStampEntry; + private NetworkTableEntry ntValidEntry; private MemoryManager memManager = new MemoryManager(125); - private int imgWidth, imgHeight; + // chameleon specific + private Pipeline currentPipeline; + private VisionProcess visionProcess; + private CameraValues camVals; - private void ChangeCameraValues(int Exposure, int Brightness) { - SettingsManager.getInstance().UsbCameras.get(CameraName).setBrightness(Brightness); - SettingsManager.getInstance().UsbCameras.get(CameraName).setExposureManual(Exposure); + // cscore + private CvSink cvSink; + private CvSource cvPublish; + + // 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 void ChangeCameraValues(int exposure, int brightness) { + SettingsManager.UsbCameras.get(CameraName).setBrightness(brightness); + SettingsManager.UsbCameras.get(CameraName).setExposureManual(exposure); } private void DriverModeListener(EntryNotification entryNotification) { @@ -51,11 +71,11 @@ public class CameraProcess implements Runnable { } } - public CameraProcess(String CameraName) { - this.CameraName = CameraName; + public CameraProcess(String cameraName) { + CameraName = cameraName; // NetworkTables - NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + CameraName); + NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraName); ntPipelineEntry = ntTable.getEntry("Pipeline"); ntDriverModeEntry = ntTable.getEntry("Driver_Mode"); ntPitchEntry = ntTable.getEntry("Pitch"); @@ -66,37 +86,77 @@ public class CameraProcess implements Runnable { ntDriverModeEntry.addListener(this::DriverModeListener, EntryListenerFlags.kUpdate); ntPipelineEntry.addListener(this::PipelineListener, EntryListenerFlags.kUpdate); ntDriverModeEntry.setBoolean(false); - ntPipelineEntry.setString(SettingsManager.CamerasCurrentPipeline.get(CameraName)); - imgWidth = SettingsManager.Cameras.get(CameraName).camVideoMode.width; - imgHeight = SettingsManager.Cameras.get(CameraName).camVideoMode.height; + ntPipelineEntry.setString(SettingsManager.CamerasCurrentPipeline.get(cameraName)); + + // camera settings + camVals = new CameraValues(SettingsManager.Cameras.get(cameraName)); + visionProcess = new VisionProcess(camVals); + + // cscore setup + CameraServer cs = CameraServer.getInstance(); + cvSink = cs.getVideo(SettingsManager.UsbCameras.get(cameraName)); + cvPublish = cs.putVideo(cameraName, camVals.ImageWidth, camVals.ImageHeight); + + } + + 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); + } + + // TODO: Separate video output, contour drawing, data output to separate function, maybe even second thread + private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) { + var pipelineResult = new 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, currentPipeline.sort_mode, currentPipeline.target_intersection, currentPipeline.target_group); + 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.IsValid = true; + if (!currentPipeline.is_calibrated) { + pipelineResult.CalibratedX = camVals.CenterX; + pipelineResult.CalibratedY = camVals.CenterY; + } else { + pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.B) / currentPipeline.M; + pipelineResult.CalibratedY = finalRect.center.x * currentPipeline.M + currentPipeline.B; + pipelineResult.Pitch = camVals.CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY); + pipelineResult.Yaw = camVals.CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX); + } + // Send calc using networktables + // TODO Send pitch yaw distance and Raw Point using websockets to client for calib calc + drawContour(outputImage, finalRect); + } + } + } + + return pipelineResult; } @Override public void run() { - // camera values - 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; - - // actual OpenCV objects - List FoundContours = new ArrayList<>(); - List FilteredContours = new ArrayList<>(); - List GroupedContours = new ArrayList<>(); - Mat inputMat = new Mat(); - Mat hsvThreshMat = new Mat(); - Mat outputMat = new Mat(); - Scalar contourColor = new Scalar(255, 0, 0); - // processing time tracking long startTime; double processTimeMs; double fps; - //camera results - double CalibratedX, CalibratedY, Pitch, Yaw; - boolean isValid; while (!Thread.interrupted()) { FoundContours.clear(); @@ -108,63 +168,24 @@ public class CameraProcess implements Runnable { // System.out.println(SettingsManager.CamerasCurrentPipeline.get(CameraName)); // start fps counter right before grabbing input frame startTime = System.nanoTime(); - cv_sink.grabFrame(inputMat); - if (inputMat.cols() == 0 && inputMat.rows() == 0) { + cvSink.grabFrame(cameraInputMat); + if (cameraInputMat.cols() == 0 && cameraInputMat.rows() == 0) { continue; } - 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)); + // get vision data + var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat); - visionProcess.HSVThreshold(inputMat, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate); - - if (currentPipeline.is_binary == 1) { - Imgproc.cvtColor(hsvThreshMat, outputMat, Imgproc.COLOR_GRAY2BGR, 3); - } else { - outputMat = inputMat; - } - - FoundContours = visionProcess.FindContours(hsvThreshMat); - if (FoundContours.size() > 0) { - FilteredContours = visionProcess.FilterContours(FoundContours, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent, currentPipeline.sort_mode, currentPipeline.target_intersection, currentPipeline.target_group); - 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); - if (!currentPipeline.is_calibrated) { - CalibratedX = camVals.CenterX; - CalibratedY = camVals.CenterY; - } else { - CalibratedX = (finalRect.center.y - currentPipeline.B) / currentPipeline.M; - CalibratedY = finalRect.center.x * currentPipeline.M + currentPipeline.B; - Pitch = camVals.CalculatePitch(finalRect.center.y, CalibratedY); - Yaw = camVals.CalculateYaw(finalRect.center.x, CalibratedX); - } - isValid = true; - // Send calc using networktables - // TODO Send pitch yaw distance and Raw Point using websockets to client for calib calc - if (finalRect != null) { - List a = new ArrayList<>(); - Point[] vertices = new Point[4]; - finalRect.points(vertices); - a.add(new MatOfPoint(vertices)); - Imgproc.drawContours(outputMat, a, 0, contourColor, 3); - } - - } else { - isValid = false; - } - } - } - - cv_publish.putFrame(outputMat); + cvPublish.putFrame(streamOutputMat); // calculate FPS after publishing output frame processTimeMs = (System.nanoTime() - startTime) * 1e-6; fps = 1000 / processTimeMs; -// System.out.printf("%s Process time: %fms, FPS: %.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n",CameraName ,processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size()); + System.out.printf("%s - Process time: %fms, FPS: %.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n",CameraName ,processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size()); - inputMat.release(); + cameraInputMat.release(); hsvThreshMat.release(); + + memManager.run(true); } } } diff --git a/Main/src/main/java/com/chameleonvision/vision/process/PipelineResult.java b/Main/src/main/java/com/chameleonvision/vision/process/PipelineResult.java new file mode 100644 index 000000000..76643cb7a --- /dev/null +++ b/Main/src/main/java/com/chameleonvision/vision/process/PipelineResult.java @@ -0,0 +1,9 @@ +package com.chameleonvision.vision.process; + +public class PipelineResult { + public boolean IsValid = false; + public double CalibratedX = 0.0; + public double CalibratedY = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; +}