mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
resolution change fix, various cleanup
This commit is contained in:
Banks Troutman
@@ -4,196 +4,192 @@ 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 org.opencv.imgproc.Imgproc;
|
||||
|
||||
import java.util.*;
|
||||
|
||||
@SuppressWarnings("WeakerAccess")
|
||||
public class CVProcess {
|
||||
|
||||
private HashMap<String, Integer>TargetGrouping= new HashMap<>() {{
|
||||
put("Single", 1);
|
||||
put("Dual", 2);
|
||||
put("Triple", 3);
|
||||
put("Quadruple", 4);
|
||||
put("Quintuple", 5);
|
||||
}};
|
||||
private final CameraValues CamVals;
|
||||
private HashMap<String, Integer> TargetGrouping = new HashMap<>() {{
|
||||
put("Single", 1);
|
||||
put("Dual", 2);
|
||||
put("Triple", 3);
|
||||
put("Quadruple", 4);
|
||||
put("Quintuple", 5);
|
||||
}};
|
||||
private Mat Kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
|
||||
private Mat hsvImage = new Mat();
|
||||
private List<MatOfPoint> FoundContours = new ArrayList<>();
|
||||
private Mat binaryMat = new Mat();
|
||||
private List<MatOfPoint> FilteredContours = new ArrayList<>();
|
||||
private Comparator<RotatedRect> SortByCentermostComparator = Comparator.comparingDouble(this::calcDistance);
|
||||
private List<RotatedRect> FinalCountours = new ArrayList<>();
|
||||
private Mat intersectMatA = new Mat();
|
||||
private Mat intersectMatB = new Mat();
|
||||
|
||||
private final CameraValues CamVals;
|
||||
CVProcess(CameraValues camVals) {
|
||||
CamVals = camVals;
|
||||
}
|
||||
|
||||
CVProcess(CameraValues camVals){
|
||||
CamVals = camVals;
|
||||
}
|
||||
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 Mat Kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
|
||||
List<MatOfPoint> 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 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();
|
||||
}
|
||||
List<MatOfPoint> FilterContours(List<MatOfPoint> InputContours, List<Integer> area, List<Integer> ratio, List<Integer> 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<MatOfPoint> FoundContours = new ArrayList<>();
|
||||
private Mat binaryMat = new Mat();
|
||||
List<MatOfPoint> 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 double calcDistance(RotatedRect rect) {
|
||||
return FastMath.sqrt(FastMath.pow(CamVals.CenterX - rect.center.x, 2) + FastMath.pow(CamVals.CenterY - rect.center.y, 2));
|
||||
}
|
||||
|
||||
private List<MatOfPoint> FilteredContours = new ArrayList<>();
|
||||
List<MatOfPoint> FilterContours(List<MatOfPoint> InputContours, List<Integer> area, List<Integer> ratio, List<Integer> 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;
|
||||
}
|
||||
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 Collections.min(inputRects, SortByCentermostComparator);
|
||||
default:
|
||||
return inputRects.get(0); // default to whatever the first contour is, but this should never happen
|
||||
}
|
||||
}
|
||||
|
||||
private double calcDistance(RotatedRect rect) {
|
||||
return FastMath.sqrt(FastMath.pow(CamVals.CenterX - rect.center.x, 2) + FastMath.pow(CamVals.CenterY - rect.center.y, 2));
|
||||
}
|
||||
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++) {
|
||||
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 Comparator<RotatedRect> SortByCentermostComparator = Comparator.comparingDouble(this::calcDistance);
|
||||
} 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;
|
||||
}
|
||||
|
||||
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 Collections.min(inputRects, SortByCentermostComparator);
|
||||
default:
|
||||
return inputRects.get(0); // default to whatever the first contour is, but this should never happen
|
||||
}
|
||||
}
|
||||
|
||||
private List<RotatedRect> FinalCountours = new ArrayList<>();
|
||||
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++){
|
||||
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;
|
||||
}
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,51 +6,60 @@ import org.opencv.core.Mat;
|
||||
|
||||
public class CameraProcess implements Runnable {
|
||||
|
||||
private final Camera camera;
|
||||
private final int maxFPS;
|
||||
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;
|
||||
|
||||
private final Object inputFrameLock = new Object();
|
||||
private final Object outputFrameLock = new Object();
|
||||
|
||||
public 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);
|
||||
}
|
||||
}
|
||||
|
||||
void updateFrame(Mat inputFrame) {
|
||||
synchronized (inputFrameLock) {
|
||||
inputFrame.copyTo(this.inputFrame);
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
long getLatestFrame(Mat outputFrame) {
|
||||
synchronized (outputFrameLock) {
|
||||
this.outputFrame.copyTo(outputFrame);
|
||||
return timestamp;
|
||||
}
|
||||
}
|
||||
void updateFrame(Mat inputFrame) {
|
||||
synchronized (inputFrameLock) {
|
||||
inputFrame.copyTo(this.inputFrame);
|
||||
}
|
||||
}
|
||||
|
||||
@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();
|
||||
}
|
||||
}
|
||||
}
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -14,227 +14,223 @@ import java.util.List;
|
||||
|
||||
public class VisionProcess implements Runnable {
|
||||
|
||||
private final Camera camera;
|
||||
private final String cameraName;
|
||||
private final Camera camera;
|
||||
private final String cameraName;
|
||||
private final CameraProcess cameraProcess;
|
||||
// NetworkTables
|
||||
private NetworkTableEntry ntPipelineEntry;
|
||||
private 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 long TimeStamp = 0;
|
||||
|
||||
// NetworkTables
|
||||
private NetworkTableEntry ntPipelineEntry;
|
||||
private NetworkTableEntry ntDriverModeEntry;
|
||||
private NetworkTableEntry ntYawEntry;
|
||||
private NetworkTableEntry ntPitchEntry;
|
||||
private NetworkTableEntry ntDistanceEntry;
|
||||
private NetworkTableEntry ntTimeStampEntry;
|
||||
private NetworkTableEntry ntValidEntry;
|
||||
public VisionProcess(Camera processCam) {
|
||||
camera = processCam;
|
||||
this.cameraName = camera.name;
|
||||
|
||||
// chameleon specific
|
||||
private Pipeline currentPipeline;
|
||||
private CVProcess cvProcess;
|
||||
// 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());
|
||||
|
||||
// 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 long TimeStamp = 0;
|
||||
// camera settings
|
||||
cvProcess = new CVProcess(camera.getCamVals());
|
||||
cameraProcess = new CameraProcess(camera);
|
||||
}
|
||||
|
||||
private final CameraProcess cameraProcess;
|
||||
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 = Integer.parseInt(entryNotification.value.getString().replace("pipeline", ""));
|
||||
if (camera.getPipelines().containsKey(ntPipelineIndex)) {
|
||||
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();
|
||||
var pipeline = camera.getCurrentPipeline();
|
||||
|
||||
camera.setExposure(pipeline.exposure);
|
||||
camera.setBrightness(pipeline.brightness);
|
||||
HashMap<String,Object> pipeChange = new HashMap<>();
|
||||
pipeChange.put("curr_pipeline",ntPipelineIndex);
|
||||
ServerHandler.broadcastMessage(pipeChange);
|
||||
camera.setExposure(pipeline.exposure);
|
||||
camera.setBrightness(pipeline.brightness);
|
||||
HashMap<String, Object> pipeChange = new HashMap<>();
|
||||
pipeChange.put("curr_pipeline", ntPipelineIndex);
|
||||
ServerHandler.broadcastMessage(pipeChange);
|
||||
|
||||
} else {
|
||||
ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex());
|
||||
}
|
||||
}
|
||||
} else {
|
||||
ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex());
|
||||
}
|
||||
}
|
||||
|
||||
public VisionProcess(Camera processCam) {
|
||||
camera = processCam;
|
||||
this.cameraName = camera.name;
|
||||
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);
|
||||
}
|
||||
|
||||
// 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());
|
||||
private void updateNetworkTables(PipelineResult pipelineResult) {
|
||||
ntValidEntry.setBoolean(pipelineResult.IsValid);
|
||||
if (pipelineResult.IsValid) {
|
||||
ntYawEntry.setNumber(pipelineResult.Yaw);
|
||||
ntPitchEntry.setNumber(pipelineResult.Pitch);
|
||||
}
|
||||
ntTimeStampEntry.setNumber(TimeStamp);
|
||||
}
|
||||
|
||||
// camera settings
|
||||
cvProcess = new CVProcess(camera.getCamVals());
|
||||
cameraProcess = new CameraProcess(camera);
|
||||
}
|
||||
private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) {
|
||||
var pipelineResult = new PipelineResult();
|
||||
|
||||
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);
|
||||
}
|
||||
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));
|
||||
|
||||
private void updateNetworkTables(PipelineResult pipelineResult) {
|
||||
ntValidEntry.setBoolean(pipelineResult.IsValid);
|
||||
if (pipelineResult.IsValid){
|
||||
ntYawEntry.setNumber(pipelineResult.Yaw);
|
||||
ntPitchEntry.setNumber(pipelineResult.Pitch);
|
||||
}
|
||||
ntTimeStampEntry.setNumber(TimeStamp);
|
||||
}
|
||||
cvProcess.HSVThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate);
|
||||
|
||||
private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) {
|
||||
var pipelineResult = new PipelineResult();
|
||||
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;
|
||||
}
|
||||
pipelineResult.Pitch = camera.getCamVals().CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY);
|
||||
pipelineResult.Yaw = camera.getCamVals().CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX);
|
||||
drawContour(outputImage, finalRect);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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));
|
||||
return pipelineResult;
|
||||
}
|
||||
|
||||
cvProcess.HSVThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate);
|
||||
@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;
|
||||
|
||||
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;
|
||||
}
|
||||
pipelineResult.Pitch = camera.getCamVals().CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY);
|
||||
pipelineResult.Yaw = camera.getCamVals().CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX);
|
||||
drawContour(outputImage, finalRect);
|
||||
}
|
||||
}
|
||||
}
|
||||
new Thread(cameraProcess).start();
|
||||
|
||||
return pipelineResult;
|
||||
}
|
||||
long lastFrameEndNanosec = 0;
|
||||
|
||||
@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;
|
||||
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();
|
||||
|
||||
new Thread(cameraProcess).start();
|
||||
// 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();
|
||||
}
|
||||
|
||||
long lastFrameEndNanosec = 0;
|
||||
currentPipeline = camera.getCurrentPipeline();
|
||||
// start fps counter right before grabbing input frame
|
||||
TimeStamp = cameraProcess.getLatestFrame(cameraInputMat);
|
||||
if (cameraInputMat.cols() == 0 && cameraInputMat.rows() == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
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();
|
||||
// get vision data
|
||||
var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat);
|
||||
updateNetworkTables(pipelineResult);
|
||||
if (cameraName.equals(SettingsManager.GeneralSettings.curr_camera)) {
|
||||
HashMap<String, Object> WebSend = new HashMap<>();
|
||||
HashMap<String, Object> point = new HashMap<>();
|
||||
List<Double> 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);
|
||||
}
|
||||
|
||||
// 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();
|
||||
}
|
||||
cameraProcess.updateFrame(streamOutputMat);
|
||||
|
||||
currentPipeline = camera.getCurrentPipeline();
|
||||
// start fps counter right before grabbing input frame
|
||||
TimeStamp = cameraProcess.getLatestFrame(cameraInputMat);
|
||||
if (cameraInputMat.cols() == 0 && cameraInputMat.rows() == 0) {
|
||||
continue;
|
||||
}
|
||||
cameraInputMat.release();
|
||||
hsvThreshMat.release();
|
||||
|
||||
// get vision data
|
||||
var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat);
|
||||
updateNetworkTables(pipelineResult);
|
||||
if (cameraName.equals(SettingsManager.GeneralSettings.curr_camera)) {
|
||||
HashMap<String, Object> WebSend = new HashMap<>();
|
||||
HashMap<String, Object> point = new HashMap<>();
|
||||
List<Double> 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);
|
||||
}
|
||||
// calculate FPS
|
||||
lastFrameEndNanosec = System.nanoTime();
|
||||
processTimeMs = (lastFrameEndNanosec - startTime) * 1e-6;
|
||||
fps = 1000 / processTimeMs;
|
||||
|
||||
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());
|
||||
}
|
||||
}
|
||||
}
|
||||
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