mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-21 01:01:41 +00:00
code cleanup and ui route bugfix
This commit is contained in:
ori agranat
@@ -18,7 +18,7 @@ public class CVProcess {
|
||||
|
||||
private final CameraValues cameraValues;
|
||||
private Mat kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
|
||||
private Size blur = new Size(2,2);
|
||||
private Size blur = new Size(2, 2);
|
||||
private Mat hsvImage = new Mat();
|
||||
private List<MatOfPoint> foundContours = new ArrayList<>();
|
||||
private Mat binaryMat = new Mat();
|
||||
@@ -58,7 +58,7 @@ public class CVProcess {
|
||||
for (MatOfPoint Contour : inputContours) {
|
||||
try {
|
||||
double contourArea = Imgproc.contourArea(Contour);
|
||||
double AreaRatio = (contourArea / cameraValues.ImageArea)*100;
|
||||
double AreaRatio = (contourArea / cameraValues.ImageArea) * 100;
|
||||
double minArea = (MathHandler.sigmoid(area.get(0)));
|
||||
double maxArea = (MathHandler.sigmoid(area.get(1)));
|
||||
if (AreaRatio < minArea || AreaRatio > maxArea) {
|
||||
@@ -89,10 +89,12 @@ public class CVProcess {
|
||||
private double calcDistance(RotatedRect rect) {
|
||||
return FastMath.sqrt(FastMath.pow(cameraValues.CenterX - rect.center.x, 2) + FastMath.pow(cameraValues.CenterY - rect.center.y, 2));
|
||||
}
|
||||
private double calcMomentsX(MatOfPoint c){
|
||||
|
||||
private double calcMomentsX(MatOfPoint c) {
|
||||
Moments m = Imgproc.moments(c);
|
||||
return (m.get_m10()/m.get_m00());
|
||||
return (m.get_m10() / m.get_m00());
|
||||
}
|
||||
|
||||
RotatedRect sortTargetsToOne(List<RotatedRect> inputRects, SortMode sortMode) {
|
||||
switch (sortMode) {
|
||||
case Largest:
|
||||
@@ -116,37 +118,34 @@ public class CVProcess {
|
||||
|
||||
List<RotatedRect> groupTargets(List<MatOfPoint> inputContours, TargetIntersection intersectionPoint, TargetGroup targetGroup) {
|
||||
finalCountours.clear();
|
||||
if (!targetGroup.equals(TargetGroup.Single)) {
|
||||
if (targetGroup.equals(TargetGroup.Dual)) {
|
||||
inputContours.sort(sortByMomentsX);
|
||||
for (var i = 0; i < inputContours.size(); i++) {
|
||||
List<Point> FinalContourList = new ArrayList<>(inputContours.get(i).toList());
|
||||
for (var c = 0; c < targetGroup.ordinal(); c++) {
|
||||
try {
|
||||
MatOfPoint firstContour = inputContours.get(i + c);
|
||||
MatOfPoint secondContour = inputContours.get(i + c + 1);
|
||||
if (isIntersecting(firstContour, secondContour, intersectionPoint)) {
|
||||
FinalContourList.addAll(secondContour.toList());
|
||||
}
|
||||
else{
|
||||
FinalContourList.clear();
|
||||
break;
|
||||
}
|
||||
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) {
|
||||
try {
|
||||
MatOfPoint firstContour = inputContours.get(i);
|
||||
MatOfPoint secondContour = inputContours.get(i + 1);
|
||||
if (isIntersecting(firstContour, secondContour, intersectionPoint)) {
|
||||
FinalContourList.addAll(secondContour.toList());
|
||||
} else {
|
||||
FinalContourList.clear();
|
||||
break;
|
||||
}
|
||||
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 {
|
||||
} else if (targetGroup.equals(TargetGroup.Single)) {
|
||||
for (MatOfPoint inputContour : inputContours) {
|
||||
MatOfPoint2f contour = new MatOfPoint2f();
|
||||
contour.fromArray(inputContour.toArray());
|
||||
@@ -174,7 +173,7 @@ public class CVProcess {
|
||||
double y0A = a.center.y;
|
||||
double x0B = b.center.x;
|
||||
double y0B = b.center.y;
|
||||
double intersectionX = ((mA * x0A) - y0A - (mB * x0B) + y0B )/ (mA - mB);
|
||||
double intersectionX = ((mA * x0A) - y0A - (mB * x0B) + y0B) / (mA - mB);
|
||||
double intersectionY = (mA * (intersectionX - x0A)) + y0A;
|
||||
double massX = (x0A + x0B) / 2;
|
||||
double massY = (y0A + y0B) / 2;
|
||||
|
||||
@@ -6,60 +6,60 @@ import org.opencv.core.Mat;
|
||||
|
||||
public class CameraProcess implements Runnable {
|
||||
|
||||
private final Camera camera;
|
||||
private final int maxFPS;
|
||||
private final Object inputFrameLock = new Object();
|
||||
private final Object outputFrameLock = new Object();
|
||||
private Mat inputFrame;
|
||||
private Mat outputFrame;
|
||||
private long timestamp;
|
||||
private final Camera camera;
|
||||
private final int maxFPS;
|
||||
private final Object inputFrameLock = new Object();
|
||||
private final Object outputFrameLock = new Object();
|
||||
private Mat inputFrame;
|
||||
private Mat outputFrame;
|
||||
private long timestamp;
|
||||
|
||||
CameraProcess(Camera camera) {
|
||||
this.camera = camera;
|
||||
maxFPS = camera.getVideoMode().fps;
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
private void updateFrameSize() {
|
||||
var camVals = camera.getCamVals();
|
||||
synchronized (inputFrameLock) {
|
||||
inputFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3);
|
||||
}
|
||||
synchronized (outputFrameLock) {
|
||||
outputFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3);
|
||||
}
|
||||
}
|
||||
private void updateFrameSize() {
|
||||
var camVals = camera.getCamVals();
|
||||
synchronized (inputFrameLock) {
|
||||
inputFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3);
|
||||
}
|
||||
synchronized (outputFrameLock) {
|
||||
outputFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3);
|
||||
}
|
||||
}
|
||||
|
||||
void updateFrame(Mat inputFrame) {
|
||||
void updateFrame(Mat inputFrame) {
|
||||
synchronized (inputFrameLock) {
|
||||
inputFrame.copyTo(this.inputFrame);
|
||||
}
|
||||
}
|
||||
|
||||
long getLatestFrame(Mat outputFrame) {
|
||||
synchronized (outputFrameLock) {
|
||||
this.outputFrame.copyTo(outputFrame);
|
||||
return timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
while (!Thread.interrupted()) {
|
||||
synchronized (outputFrameLock) {
|
||||
timestamp = camera.grabFrame(outputFrame);
|
||||
}
|
||||
synchronized (inputFrameLock) {
|
||||
inputFrame.copyTo(this.inputFrame);
|
||||
}
|
||||
}
|
||||
|
||||
long getLatestFrame(Mat outputFrame) {
|
||||
synchronized (outputFrameLock) {
|
||||
this.outputFrame.copyTo(outputFrame);
|
||||
return timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
camera.putFrame(inputFrame);
|
||||
}
|
||||
var msToWait = (long) 1000 / maxFPS;
|
||||
try {
|
||||
Thread.sleep(msToWait);
|
||||
} catch (InterruptedException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3,11 +3,11 @@ package com.chameleonvision.vision.process;
|
||||
import org.opencv.core.RotatedRect;
|
||||
|
||||
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;
|
||||
public double Area = 0.0;
|
||||
RotatedRect RawPoint;
|
||||
public boolean IsValid = false;
|
||||
public double CalibratedX = 0.0;
|
||||
public double CalibratedY = 0.0;
|
||||
public double Pitch = 0.0;
|
||||
public double Yaw = 0.0;
|
||||
public double Area = 0.0;
|
||||
RotatedRect RawPoint;
|
||||
}
|
||||
|
||||
@@ -16,248 +16,247 @@ import java.util.List;
|
||||
|
||||
public class VisionProcess implements Runnable {
|
||||
|
||||
private final Camera camera;
|
||||
private final String cameraName;
|
||||
private final CameraProcess cameraProcess;
|
||||
// NetworkTables
|
||||
public NetworkTableEntry ntPipelineEntry;
|
||||
public NetworkTableEntry ntDriverModeEntry;
|
||||
private NetworkTableEntry ntYawEntry;
|
||||
private NetworkTableEntry ntPitchEntry;
|
||||
private NetworkTableEntry ntDistanceEntry;
|
||||
private NetworkTableEntry ntTimeStampEntry;
|
||||
private NetworkTableEntry ntValidEntry;
|
||||
// chameleon specific
|
||||
private Pipeline currentPipeline;
|
||||
private CVProcess cvProcess;
|
||||
// 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 Scalar BoxRectColor = new Scalar(0, 0, 233);
|
||||
private long timeStamp = 0;
|
||||
private final Camera camera;
|
||||
private final String cameraName;
|
||||
private final CameraProcess cameraProcess;
|
||||
// NetworkTables
|
||||
public NetworkTableEntry ntPipelineEntry;
|
||||
public NetworkTableEntry ntDriverModeEntry;
|
||||
private NetworkTableEntry ntYawEntry;
|
||||
private NetworkTableEntry ntPitchEntry;
|
||||
private NetworkTableEntry ntDistanceEntry;
|
||||
private NetworkTableEntry ntTimeStampEntry;
|
||||
private NetworkTableEntry ntValidEntry;
|
||||
// chameleon specific
|
||||
private Pipeline currentPipeline;
|
||||
private CVProcess cvProcess;
|
||||
// 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 Scalar BoxRectColor = new Scalar(0, 0, 233);
|
||||
private long timeStamp = 0;
|
||||
|
||||
public VisionProcess(Camera processCam) {
|
||||
camera = processCam;
|
||||
this.cameraName = camera.name;
|
||||
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("is_valid");
|
||||
ntDriverModeEntry.addListener(this::driverModeListener, EntryListenerFlags.kUpdate);
|
||||
ntPipelineEntry.addListener(this::pipelineListener, EntryListenerFlags.kUpdate);
|
||||
ntDriverModeEntry.setBoolean(false);
|
||||
ntPipelineEntry.setNumber(camera.getCurrentPipelineIndex());
|
||||
// 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("is_valid");
|
||||
ntDriverModeEntry.addListener(this::driverModeListener, EntryListenerFlags.kUpdate);
|
||||
ntPipelineEntry.addListener(this::pipelineListener, EntryListenerFlags.kUpdate);
|
||||
ntDriverModeEntry.setBoolean(false);
|
||||
ntPipelineEntry.setNumber(camera.getCurrentPipelineIndex());
|
||||
|
||||
// camera settings
|
||||
cvProcess = new CVProcess(camera.getCamVals());
|
||||
cameraProcess = new CameraProcess(camera);
|
||||
}
|
||||
// camera settings
|
||||
cvProcess = new CVProcess(camera.getCamVals());
|
||||
cameraProcess = new CameraProcess(camera);
|
||||
}
|
||||
|
||||
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 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 void pipelineListener(EntryNotification entryNotification) {
|
||||
var ntPipelineIndex = (int) entryNotification.value.getDouble();
|
||||
if (camera.getPipelines().containsKey(ntPipelineIndex)) {
|
||||
private void pipelineListener(EntryNotification entryNotification) {
|
||||
var ntPipelineIndex = (int) entryNotification.value.getDouble();
|
||||
if (camera.getPipelines().containsKey(ntPipelineIndex)) {
|
||||
// camera.setEntryNotification.value.getString());
|
||||
var pipeline = camera.getCurrentPipeline();
|
||||
camera.setCurrentPipelineIndex(ntPipelineIndex);
|
||||
try{
|
||||
camera.setExposure(pipeline.exposure);
|
||||
}
|
||||
catch (VideoException e){
|
||||
System.err.println(e.toString());
|
||||
}
|
||||
camera.setBrightness(pipeline.brightness);
|
||||
if (SettingsManager.GeneralSettings.currentCamera.equals(cameraName)){
|
||||
SettingsManager.GeneralSettings.currentPipeline = ntPipelineIndex;
|
||||
HashMap<String, Object> pipeChange = new HashMap<>();
|
||||
pipeChange.put("currentPipeline", ntPipelineIndex);
|
||||
ServerHandler.broadcastMessage(pipeChange);
|
||||
ServerHandler.sendFullSettings();
|
||||
var pipeline = camera.getCurrentPipeline();
|
||||
camera.setCurrentPipelineIndex(ntPipelineIndex);
|
||||
try {
|
||||
camera.setExposure(pipeline.exposure);
|
||||
} catch (VideoException e) {
|
||||
System.err.println(e.toString());
|
||||
}
|
||||
camera.setBrightness(pipeline.brightness);
|
||||
if (SettingsManager.GeneralSettings.currentCamera.equals(cameraName)) {
|
||||
SettingsManager.GeneralSettings.currentPipeline = ntPipelineIndex;
|
||||
HashMap<String, Object> pipeChange = new HashMap<>();
|
||||
pipeChange.put("currentPipeline", ntPipelineIndex);
|
||||
ServerHandler.broadcastMessage(pipeChange);
|
||||
ServerHandler.sendFullSettings();
|
||||
|
||||
}
|
||||
} else {
|
||||
ntPipelineEntry.setNumber(camera.getCurrentPipelineIndex());
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
ntPipelineEntry.setNumber(camera.getCurrentPipelineIndex());
|
||||
}
|
||||
}
|
||||
|
||||
private void drawContour(Mat inputMat, RotatedRect contourRect) {
|
||||
if (contourRect == null) return;
|
||||
List<MatOfPoint> 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 drawContour(Mat inputMat, RotatedRect contourRect) {
|
||||
if (contourRect == null) return;
|
||||
List<MatOfPoint> 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);
|
||||
ntYawEntry.setNumber(pipelineResult.Yaw);
|
||||
ntPitchEntry.setNumber(pipelineResult.Pitch);
|
||||
ntDistanceEntry.setNumber(pipelineResult.Area);
|
||||
ntTimeStampEntry.setNumber(timeStamp);
|
||||
NetworkTableInstance.getDefault().flush();
|
||||
} else {
|
||||
ntYawEntry.setNumber(0.0);
|
||||
ntPitchEntry.setNumber(0.0);
|
||||
ntDistanceEntry.setNumber(0.0);
|
||||
ntTimeStampEntry.setNumber(timeStamp);
|
||||
ntValidEntry.setBoolean(false);
|
||||
}
|
||||
}
|
||||
private void updateNetworkTables(PipelineResult pipelineResult) {
|
||||
if (pipelineResult.IsValid) {
|
||||
ntValidEntry.setBoolean(true);
|
||||
ntYawEntry.setNumber(pipelineResult.Yaw);
|
||||
ntPitchEntry.setNumber(pipelineResult.Pitch);
|
||||
ntDistanceEntry.setNumber(pipelineResult.Area);
|
||||
ntTimeStampEntry.setNumber(timeStamp);
|
||||
NetworkTableInstance.getDefault().flush();
|
||||
} else {
|
||||
ntYawEntry.setNumber(0.0);
|
||||
ntPitchEntry.setNumber(0.0);
|
||||
ntDistanceEntry.setNumber(0.0);
|
||||
ntTimeStampEntry.setNumber(timeStamp);
|
||||
ntValidEntry.setBoolean(false);
|
||||
}
|
||||
}
|
||||
|
||||
private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) {
|
||||
var pipelineResult = new PipelineResult();
|
||||
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 (ntDriverModeEntry.getBoolean(false)) {
|
||||
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());
|
||||
if (currentPipeline == null) {
|
||||
return pipelineResult;
|
||||
}
|
||||
if (currentPipeline.orientation.equals(Orientation.Inverted)) {
|
||||
Core.flip(inputImage, inputImage, -1);
|
||||
}
|
||||
if (ntDriverModeEntry.getBoolean(false)) {
|
||||
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);
|
||||
cvProcess.hsvThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate);
|
||||
|
||||
if (currentPipeline.isBinary == true) {
|
||||
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.targetIntersection, currentPipeline.targetGroup);
|
||||
if (groupedContours.size() > 0) {
|
||||
var finalRect = cvProcess.sortTargetsToOne(groupedContours, currentPipeline.sortMode);
|
||||
if (currentPipeline.isBinary == true) {
|
||||
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.targetIntersection, currentPipeline.targetGroup);
|
||||
if (groupedContours.size() > 0) {
|
||||
var finalRect = cvProcess.sortTargetsToOne(groupedContours, currentPipeline.sortMode);
|
||||
// System.out.printf("Largest Contour Area: %.2f\n", finalRect.size.area());
|
||||
pipelineResult.RawPoint = finalRect;
|
||||
pipelineResult.IsValid = true;
|
||||
if (!currentPipeline.isCalibrated) {
|
||||
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);
|
||||
pipelineResult.Area = finalRect.size.area();
|
||||
drawContour(outputImage, finalRect);
|
||||
}
|
||||
}
|
||||
}
|
||||
pipelineResult.RawPoint = finalRect;
|
||||
pipelineResult.IsValid = true;
|
||||
if (!currentPipeline.isCalibrated) {
|
||||
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);
|
||||
pipelineResult.Area = finalRect.size.area();
|
||||
drawContour(outputImage, finalRect);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return pipelineResult;
|
||||
}
|
||||
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;
|
||||
@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();
|
||||
new Thread(cameraProcess).start();
|
||||
|
||||
long lastFrameEndNanosec = 0;
|
||||
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();
|
||||
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();
|
||||
}
|
||||
// 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;
|
||||
}
|
||||
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.currentCamera)) {
|
||||
HashMap<String, Object> WebSend = new HashMap<>();
|
||||
HashMap<String, Object> point = new HashMap<>();
|
||||
HashMap<String, Object> calculated = new HashMap<>();
|
||||
List<Double> center = new ArrayList<>();
|
||||
if (pipelineResult.IsValid) {
|
||||
center.add(pipelineResult.RawPoint.center.x);
|
||||
center.add(pipelineResult.RawPoint.center.y);
|
||||
calculated.put("pitch", pipelineResult.Pitch);
|
||||
calculated.put("yaw", pipelineResult.Yaw);
|
||||
} else {
|
||||
center.add(0.0);
|
||||
center.add(0.0);
|
||||
calculated.put("pitch", 0);
|
||||
calculated.put("yaw", 0);
|
||||
}
|
||||
point.put("fps", uiFps);
|
||||
point.put("calculated",calculated);
|
||||
point.put("rawPoint",center);
|
||||
WebSend.put("point", point);
|
||||
ServerHandler.broadcastMessage(WebSend);
|
||||
}
|
||||
// get vision data
|
||||
var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat);
|
||||
updateNetworkTables(pipelineResult);
|
||||
if (cameraName.equals(SettingsManager.GeneralSettings.currentCamera)) {
|
||||
HashMap<String, Object> WebSend = new HashMap<>();
|
||||
HashMap<String, Object> point = new HashMap<>();
|
||||
HashMap<String, Object> calculated = new HashMap<>();
|
||||
List<Double> center = new ArrayList<>();
|
||||
if (pipelineResult.IsValid) {
|
||||
center.add(pipelineResult.RawPoint.center.x);
|
||||
center.add(pipelineResult.RawPoint.center.y);
|
||||
calculated.put("pitch", pipelineResult.Pitch);
|
||||
calculated.put("yaw", pipelineResult.Yaw);
|
||||
} else {
|
||||
center.add(0.0);
|
||||
center.add(0.0);
|
||||
calculated.put("pitch", 0);
|
||||
calculated.put("yaw", 0);
|
||||
}
|
||||
point.put("fps", uiFps);
|
||||
point.put("calculated", calculated);
|
||||
point.put("rawPoint", center);
|
||||
WebSend.put("point", point);
|
||||
ServerHandler.broadcastMessage(WebSend);
|
||||
}
|
||||
|
||||
cameraProcess.updateFrame(streamOutputMat);
|
||||
cameraProcess.updateFrame(streamOutputMat);
|
||||
|
||||
cameraInputMat.release();
|
||||
hsvThreshMat.release();
|
||||
cameraInputMat.release();
|
||||
hsvThreshMat.release();
|
||||
|
||||
// calculate FPS
|
||||
lastFrameEndNanosec = System.nanoTime();
|
||||
processTimeMs = (lastFrameEndNanosec - startTime) * 1e-6;
|
||||
fps = 1000 / processTimeMs;
|
||||
//please dont enable if you are not debugging
|
||||
// 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());
|
||||
}
|
||||
}
|
||||
}
|
||||
// calculate FPS
|
||||
lastFrameEndNanosec = System.nanoTime();
|
||||
processTimeMs = (lastFrameEndNanosec - startTime) * 1e-6;
|
||||
fps = 1000 / processTimeMs;
|
||||
//please dont enable if you are not debugging
|
||||
// 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());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user