Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added SortMode rect filtering, Added exposure/brightness websocket adjustment

Browse Source
This commit is contained in:
Banks Troutman
2019-09-17 02:12:53 -04:00
parent 0bbbb8049d
commit 6187b1a205
4 changed files with 163 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java
View File

@@ -8,6 +8,7 @@ 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.apache.commons.math3.stat.descriptive.moment.Mean;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
@@ -25,7 +26,7 @@ public class CameraProcess implements Runnable {
private int imgWidth, imgHeight;
public CameraProcess(String CameraName){
public CameraProcess(String CameraName) {
this.CameraName = CameraName;
// add pipeline
@@ -34,16 +35,15 @@ public class CameraProcess implements Runnable {
// NetworkTables
NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + CameraName);
ntPipelineEntry = ntTable.getEntry("Pipeline");
ntDriverModeEntry = ntTable.getEntry("Driver_Mode");
ntDriverModeEntry = ntTable.getEntry("Driver_Mode");
imgWidth = SettingsManager.Cameras.get(CameraName).camVideoMode.width;
imgHeight = SettingsManager.Cameras.get(CameraName).camVideoMode.height;
}
@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;
@@ -51,69 +51,72 @@ public class CameraProcess implements Runnable {
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 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;
startTime = System.nanoTime();
int duration = 1;
int counter = 0;
double fps = 0;
while (!Thread.interrupted()) {
// processing time tracking
long startTime;
double processTimeMs;
double fps;
while (!Thread.interrupted()) {
FoundContours.clear();
FilteredContours.clear();
GroupedContours.clear();
currentPipeline = SettingsManager.Cameras.get(CameraName).pipelines.get(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) { continue; }
// Imgproc.cvtColor(inputMat, bgrMat, Imgproc.COLOR_RGB2BGR, 3);
if (inputMat.cols() == 0 && inputMat.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));
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);
GroupedContours = visionProcess.GroupTargets(FilteredContours,currentPipeline.target_intersection,currentPipeline.target_group);
if (currentPipeline.is_binary == 1) {
Imgproc.cvtColor(hsvThreshMat, hsvThreshMat, Imgproc.COLOR_GRAY2BGR, 3);
outputMat = hsvThreshMat;
Imgproc.cvtColor(hsvThreshMat, outputMat, Imgproc.COLOR_GRAY2BGR, 3);
} else {
outputMat = inputMat;
}
if (GroupedContours.size() > 0) {
List<MatOfPoint> a = new ArrayList<>();
Point[] vertices = new Point[4];
GroupedContours.get(0).points(vertices);
a.add(new MatOfPoint(vertices));
Imgproc.drawContours(outputMat,a, 0, contourColor, 3);
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 (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);
}
}
}
}
cv_publish.putFrame(outputMat);
System.out.println("fps: " + fps);
// calculate FPS after publishing output frame
processTimeMs = (System.nanoTime() - startTime) * 1e-6;
fps = 1000 / processTimeMs;
System.out.printf("Process time: %fms, FPS: %.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n", processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size());
inputMat.release();
hsvThreshMat.release();
for (MatOfPoint oldMat : FoundContours) { oldMat.release(); }
for (MatOfPoint oldMat1 : FilteredContours) { oldMat1.release(); }
memManager.run();
counter++;
if ((System.nanoTime() - startTime)*1e-9 > duration){
fps = (counter / ((System.nanoTime() - startTime)*1e-9 ));
counter = 0;
startTime = System.nanoTime();
}
}
}
}

88
Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java
View File

@@ -1,13 +1,13 @@
package com.chameleonvision.vision.process;
import com.chameleonvision.vision.CameraValues;
import org.apache.commons.math3.util.FastMath;
import org.jetbrains.annotations.NotNull;
import org.opencv.core.*;
import org.opencv.imgproc.*;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.*;
import java.util.stream.Collectors;
public class VisionProcess {
@@ -28,7 +28,6 @@ public class VisionProcess {
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);
@@ -61,7 +60,7 @@ public class VisionProcess {
continue;
}
var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray()));
var targetFullness = (contourArea/rect.size.area())*100;
var targetFullness = (contourArea / rect.size.area()) * 100;
if (targetFullness <= extent.get(0) || targetArea >= extent.get(1)){
continue;
}
@@ -76,16 +75,81 @@ public class VisionProcess {
return FilteredContours;
}
private static Comparator<RotatedRect> SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.size.area(), rect1.size.area());
private static Comparator<RotatedRect> SortBySmallestComparator = SortByLargestComparator.reversed();
private static Comparator<RotatedRect> SortByHighestComparator = (rect1, rect2) -> Double.compare(rect2.center.y, rect1.center.y);
private static Comparator<RotatedRect> SortByLowestComparator = SortByHighestComparator.reversed();
private static Comparator<RotatedRect> SortByLeftmostComparator = Comparator.comparingDouble(rect -> rect.center.x);
private static Comparator<RotatedRect> SortByRightmostComparator = SortByLeftmostComparator.reversed();
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<RotatedRect> SortByCentermostComparator = Comparator.comparingDouble(this::calcDistance);
RotatedRect SortTargetsToOne(List<RotatedRect> 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 inputRects.stream().sorted(SortByCentermostComparator).collect(Collectors.toList()).get(0);
default:
return inputRects.get(0); // default to whatever the first contour is, but this should never happen
}
}
void SortTargets(List<RotatedRect> inputRects, String sortMode) {
switch (sortMode) {
case "Largest":
inputRects.sort(SortByLargestComparator);
break;
case "Smallest":
inputRects.sort(SortBySmallestComparator);
break;
case "Highest":
inputRects.sort(SortByHighestComparator);
break;
case "Lowest":
inputRects.sort(SortByLowestComparator);
break;
case "Leftmost":
inputRects.sort(SortByLeftmostComparator);
break;
case "Rightmost":
inputRects.sort(SortByRightmostComparator);
break;
case "Centermost":
inputRects.sort(SortByCentermostComparator);
break;
default:
break;
}
}
private List<RotatedRect> FinalCountours = new ArrayList<>();
public List<RotatedRect> GroupTargets(List<MatOfPoint> InputContours, String IntersectionPoint, String TargetGroup) {
List<RotatedRect> GroupTargets(List<MatOfPoint> InputContours, String IntersectionPoint, String TargetGroup) {
FinalCountours.clear();
if (!TargetGroup.equals("Single")){
for (var i = 0; i < InputContours.size(); i++){
List<Point> FinalContourList = new ArrayList<>(InputContours.get(i).toList());
for (var c = 0; c < (TargetGrouping.get(TargetGroup)-1);c++){
for (var c = 0; c < (TargetGrouping.get(TargetGroup) - 1); c++){
try{
MatOfPoint firstContour = InputContours.get(i + c);
MatOfPoint secondContour = InputContours.get(i+c+1);
MatOfPoint secondContour = InputContours.get(i + c + 1);
if (IsIntersecting(firstContour, secondContour, IntersectionPoint)){
FinalContourList.addAll(secondContour.toList());
}
@@ -93,7 +157,7 @@ public class VisionProcess {
secondContour.release();
MatOfPoint2f contour = new MatOfPoint2f();
contour.fromList(FinalContourList);
if (contour.cols() !=0 && contour.rows() != 0){
if (contour.cols() != 0 && contour.rows() != 0){
RotatedRect rect = Imgproc.minAreaRect(contour);
FinalCountours.add(rect);
}
@@ -105,10 +169,10 @@ public class VisionProcess {
}
} else {
for (var i = 0; i < InputContours.size(); i++){
for (MatOfPoint inputContour : InputContours) {
MatOfPoint2f contour = new MatOfPoint2f();
contour.fromArray(InputContours.get(i).toArray());
if (contour.cols() !=0 && contour.rows() != 0) {
contour.fromArray(inputContour.toArray());
if (contour.cols() != 0 && contour.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contour);
FinalCountours.add(rect);
}