mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Separated vision process loop and added PipelineResult class
This commit is contained in:
Banks Troutman
@@ -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<MatOfPoint> FoundContours = new ArrayList<>();
|
||||
private List<MatOfPoint> FilteredContours = new ArrayList<>();
|
||||
private List<RotatedRect> 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<MatOfPoint> 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<MatOfPoint> FoundContours = new ArrayList<>();
|
||||
List<MatOfPoint> FilteredContours = new ArrayList<>();
|
||||
List<RotatedRect> 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<MatOfPoint> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user