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

resolution change fix, various cleanup

Browse Source
This commit is contained in:
Banks Troutman
2019-09-22 02:49:30 -04:00
parent 3d1de034c7
commit 8c16815305
10 changed files with 632 additions and 594 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
Main/src/main/java/com/chameleonvision/Main.java
View File

@@ -6,16 +6,16 @@ import com.chameleonvision.vision.process.VisionProcess;
import com.chameleonvision.web.Server;
public class Main {
public static void main(String[] args) {
if (CameraManager.initializeCameras()) {
SettingsManager.initialize();
for (var camSet : CameraManager.getAllCamerasByName().entrySet()) {
new Thread(new VisionProcess(camSet.getValue())).start();
}
// NetworkTableInstance.getDefault().startClientTeam(SettingsManager.GeneralSettings.team_number);
Server.main(8888);
} else {
System.err.println("No cameras connected!");
}
}
public static void main(String[] args) {
if (CameraManager.initializeCameras()) {
SettingsManager.initialize();
for (var camSet : CameraManager.getAllCamerasByName().entrySet()) {
new Thread(new VisionProcess(camSet.getValue())).start();
}
// NetworkTableInstance.getDefault().startClientTeam(SettingsManager.GeneralSettings.team_number);
Server.main(8888);
} else {
System.err.println("No cameras connected!");
}
}
}

16
Main/src/main/java/com/chameleonvision/vision/GeneralSettings.java
View File

@@ -1,12 +1,12 @@
package com.chameleonvision.vision;
public class GeneralSettings {
public int team_number = 1577;
public String connection_type = "DHCP";
public String ip = "";
public String gateway = "";
public String netmask = "";
public String hostname = "Chameleon-vision";
public String curr_camera = "";
public Integer curr_pipeline = null;
public int team_number = 1577;
public String connection_type = "DHCP";
public String ip = "";
public String gateway = "";
public String netmask = "";
public String hostname = "Chameleon-vision";
public String curr_camera = "";
public Integer curr_pipeline = null;
}

37
Main/src/main/java/com/chameleonvision/vision/Pipeline.java
View File

@@ -1,24 +1,25 @@
package com.chameleonvision.vision;
import java.util.Arrays;
import java.util.List;
public class Pipeline {
public int exposure = 50;
public int brightness = 50;
public String orientation = "Normal";
public List<Integer> hue = Arrays.asList(50,180);
public List<Integer> saturation = Arrays.asList(50,255);
public List<Integer> value = Arrays.asList(50,255);
public boolean erode = false;
public boolean dilate = false;
public List<Integer> area = Arrays.asList(0,100);
public List<Integer> ratio = Arrays.asList(0,20);
public List<Integer> extent = Arrays.asList(0,100);
public int is_binary = 0;
public String sort_mode = "Largest";
public String target_group = "Single";
public String target_intersection = "Up";
public double M = 1;
public double B = 0;
public boolean is_calibrated = false;
public int exposure = 50;
public int brightness = 50;
public String orientation = "Normal";
public List<Integer> hue = Arrays.asList(50, 180);
public List<Integer> saturation = Arrays.asList(50, 255);
public List<Integer> value = Arrays.asList(50, 255);
public boolean erode = false;
public boolean dilate = false;
public List<Integer> area = Arrays.asList(0, 100);
public List<Integer> ratio = Arrays.asList(0, 20);
public List<Integer> extent = Arrays.asList(0, 100);
public int is_binary = 0;
public String sort_mode = "Largest";
public String target_group = "Single";
public String target_intersection = "Up";
public double M = 1;
public double B = 0;
public boolean is_calibrated = false;
}

59
Main/src/main/java/com/chameleonvision/vision/camera/CamVideoMode.java
View File

@@ -4,23 +4,50 @@ import edu.wpi.cscore.VideoMode;
@SuppressWarnings("WeakerAccess")
public class CamVideoMode {
public final int fps;
public final int width;
public final int height;
public final String pixel_format;
public final int fps;
public final int width;
public final int height;
public final String pixel_format;
public CamVideoMode(VideoMode videoMode) {
fps = videoMode.fps;
width = videoMode.width;
height = videoMode.height;
pixel_format = videoMode.pixelFormat.name();
}
public CamVideoMode(VideoMode videoMode) {
fps = videoMode.fps;
width = videoMode.width;
height = videoMode.height;
pixel_format = videoMode.pixelFormat.name();
}
public VideoMode.PixelFormat getActualPixelFormat() {
return VideoMode.PixelFormat.valueOf(pixel_format);
}
public VideoMode.PixelFormat getActualPixelFormat() {
return VideoMode.PixelFormat.valueOf(pixel_format);
}
public boolean isEqualToVideoMode(VideoMode videoMode) {
return videoMode.fps == fps && videoMode.width == width && videoMode.height == height && videoMode.pixelFormat == getActualPixelFormat();
}
public boolean isEqualToVideoMode(VideoMode videoMode) {
return videoMode.fps == fps && videoMode.width == width && videoMode.height == height && videoMode.pixelFormat == getActualPixelFormat();
}
public boolean equals(VideoMode vm) {
return vm.fps == fps &&
vm.width == width &&
vm.height == height &&
vm.pixelFormat == getActualPixelFormat();
}
@Override
public boolean equals(Object obj) {
if (obj == this) {
return true;
}
if (obj instanceof CamVideoMode) {
var cvm = (CamVideoMode) obj;
return cvm.fps == fps &&
cvm.width == width &&
cvm.height == height &&
cvm.pixel_format.equals(pixel_format);
} else if (obj instanceof VideoMode) {
var vm = (VideoMode) obj;
return equals(vm);
} else {
return false;
}
}
}

53
Main/src/main/java/com/chameleonvision/vision/camera/Camera.java
View File

@@ -1,16 +1,21 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.vision.Pipeline;
import com.chameleonvision.web.ServerHandler;
import edu.wpi.cscore.*;
import edu.wpi.first.cameraserver.CameraServer;
import org.opencv.core.Mat;
import java.util.Arrays;
import java.util.HashMap;
import java.util.stream.IntStream;
public class Camera {
private static double defaultFOV = 60.8;
private static final double DEFAULT_FOV = 60.8;
private static final int MINIMUM_FPS = 30;
private static final int MINIMUM_WIDTH = 320;
private static final int MINIMUM_HEIGHT = 240;
public final String name;
public final String path;
@@ -20,25 +25,21 @@ public class Camera {
private final CameraServer cs = CameraServer.getInstance();
private final CvSink cvSink;
private final Object cvSourceLock = new Object();
private CvSource cvSource;
private double FOV;
private CameraValues camVals;
private CamVideoMode camVideoMode;
private int currentPipelineIndex;
private HashMap<Integer, Pipeline> pipelines;
private final Object cvSourceLock = new Object();
public Camera(String cameraName) {
this(cameraName, defaultFOV);
this(cameraName, DEFAULT_FOV);
}
public Camera(UsbCameraInfo usbCamInfo) {
this(usbCamInfo, defaultFOV);
this(usbCamInfo, DEFAULT_FOV);
}
public Camera(String cameraName, double fov) {
@@ -62,8 +63,8 @@ public class Camera {
this.pipelines = pipelines;
// set up video mode
availableVideoModes = UsbCam.enumerateVideoModes();
// set up video modes according to minimums
availableVideoModes = Arrays.stream(UsbCam.enumerateVideoModes()).filter(v -> v.fps >= MINIMUM_FPS && v.width >= MINIMUM_WIDTH && v.height >= MINIMUM_HEIGHT).toArray(VideoMode[]::new);
setCamVideoMode(new CamVideoMode(availableVideoModes[0]));
cvSink = cs.getVideo(UsbCam);
@@ -72,8 +73,17 @@ public class Camera {
CameraManager.CameraPorts.put(name, s.getPort());
}
public VideoMode[] getAvailableVideoModes() {
return availableVideoModes;
}
public int getStreamPort() {
var s = (MjpegServer) cs.getServer("serve_" + name);
return s.getPort();
}
public void setCamVideoMode(int videoMode) {
setCamVideoMode(UsbCam.enumerateVideoModes()[videoMode]);
setCamVideoMode(availableVideoModes[videoMode]);
}
private void setCamVideoMode(VideoMode videoMode) {
@@ -89,10 +99,12 @@ public class Camera {
// update camera values
camVals = new CameraValues(this);
if ( prevVideoMode != null && prevVideoMode.width != newVideoMode.width && prevVideoMode.height != newVideoMode.height) { // if resolution changed
if (prevVideoMode != null && !prevVideoMode.equals(newVideoMode)) { // if resolution changed
synchronized (cvSourceLock) {
cvSource = cs.putVideo(name, newVideoMode.width, newVideoMode.height);
cvSource = cs.putVideo(name, newVideoMode.width, newVideoMode.height);
}
ServerHandler.sendFullSettings();
// ServerHandler.broadcastMessage(new HashMap<String, Object>(){}.put("port", getStreamPort()));
}
}
@@ -117,6 +129,7 @@ public class Camera {
if (pipelineNumber - 1 > pipelines.size()) return;
currentPipelineIndex = pipelineNumber;
}
public HashMap<Integer, Pipeline> getPipelines() {
return pipelines;
}
@@ -127,7 +140,7 @@ public class Camera {
public int getVideoModeIndex() {
return IntStream.range(0, availableVideoModes.length)
.filter(i -> camVideoMode.isEqualToVideoMode(availableVideoModes[i]))
.filter(i -> camVideoMode.equals(availableVideoModes[i]))
.findFirst()
.orElse(-1);
}
@@ -156,16 +169,16 @@ public class Camera {
}
public long grabFrame(Mat image) {
return cvSink.grabFrame(image);
}
return cvSink.grabFrame(image);
}
public CameraValues getCamVals() {
public CameraValues getCamVals() {
return camVals;
}
public void putFrame(Mat image) {
synchronized(cvSourceLock) {
public void putFrame(Mat image) {
synchronized (cvSourceLock) {
cvSource.putFrame(image);
}
}
}
}

200
Main/src/main/java/com/chameleonvision/vision/camera/CameraManager.java
View File

@@ -1,7 +1,7 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.FileHelper;
import com.chameleonvision.CameraException;
import com.chameleonvision.FileHelper;
import com.chameleonvision.settings.SettingsManager;
import com.chameleonvision.vision.Pipeline;
import com.google.gson.Gson;
@@ -19,115 +19,111 @@ import java.util.List;
public class CameraManager {
private static final Path CamConfigPath = Paths.get(SettingsManager.SettingsPath.toString(), "Cams");
private static final Path CamConfigPath = Paths.get(SettingsManager.SettingsPath.toString(), "Cams");
public static HashMap<String, Integer> CameraPorts = new HashMap<>();
static HashMap<String, UsbCameraInfo> AllUsbCameraInfosByName = new HashMap<>() {{
var suffix = 0;
for (var info : UsbCamera.enumerateUsbCameras()) {
var cap = new VideoCapture(info.dev);
if (cap.isOpened()) {
cap.release();
var name = info.name;
while (this.containsKey(name)) {
suffix++;
name = String.format("%s(%s)", info.name, suffix);
}
put(name, info);
}
}
}};
private static HashMap<String, Camera> AllCamerasByName = new HashMap<>();
static HashMap<String, UsbCameraInfo> AllUsbCameraInfosByName = new HashMap<>() {{
var suffix = 0;
for (var info : UsbCamera.enumerateUsbCameras()) {
var cap = new VideoCapture(info.dev);
if (cap.isOpened()) {
cap.release();
var name = info.name;
while (this.containsKey(name)) {
suffix++;
name = String.format("%s(%s)", info.name, suffix);
}
put(name, info);
}
}
}};
public static HashMap<String, Camera> getAllCamerasByName() {
return AllCamerasByName;
}
private static HashMap<String, Camera> AllCamerasByName = new HashMap<>();
public static boolean initializeCameras() {
if (AllUsbCameraInfosByName.size() == 0) return false;
FileHelper.CheckPath(CamConfigPath);
for (var entry : AllUsbCameraInfosByName.entrySet()) {
var camPath = Paths.get(CamConfigPath.toString(), String.format("%s.json", entry.getKey()));
File camJsonFile = new File(camPath.toString());
if (camJsonFile.exists() && camJsonFile.length() != 0) {
try {
Gson gson = new GsonBuilder().registerTypeAdapter(Camera.class, new CameraDeserializer()).create();
var camJsonFileReader = new FileReader(camPath.toString());
var gsonRead = gson.fromJson(camJsonFileReader, Camera.class);
AllCamerasByName.put(entry.getKey(), gsonRead);
} catch (FileNotFoundException ex) {
ex.printStackTrace();
}
} else {
if (!addCamera(new Camera(entry.getKey()), entry.getKey())) {
System.err.println("Failed to add camera! Already exists!");
}
}
}
return true;
}
public static HashMap<String, Camera> getAllCamerasByName() {
return AllCamerasByName;
}
private static boolean addCamera(Camera camera, String cameraName) {
if (AllCamerasByName.containsKey(cameraName)) return false;
camera.addPipeline();
AllCamerasByName.put(cameraName, camera);
return true;
}
public static HashMap<String, Integer> CameraPorts = new HashMap<>();
private static Camera getCamera(String cameraName) {
return AllCamerasByName.get(cameraName);
}
public static boolean initializeCameras() {
if (AllUsbCameraInfosByName.size() == 0) return false;
FileHelper.CheckPath(CamConfigPath);
for (var entry : AllUsbCameraInfosByName.entrySet()) {
var camPath = Paths.get(CamConfigPath.toString(), String.format("%s.json", entry.getKey()));
File camJsonFile = new File(camPath.toString());
if (camJsonFile.exists() && camJsonFile.length() != 0) {
try {
Gson gson = new GsonBuilder().registerTypeAdapter(Camera.class, new CameraDeserializer()).create();
var camJsonFileReader = new FileReader(camPath.toString());
var gsonRead = gson.fromJson(camJsonFileReader, Camera.class);
AllCamerasByName.put(entry.getKey(), gsonRead);
} catch (FileNotFoundException ex) {
ex.printStackTrace();
}
} else {
if (!addCamera(new Camera(entry.getKey()), entry.getKey())) {
System.err.println("Failed to add camera! Already exists!");
}
}
}
return true;
}
public static Camera getCurrentCamera() throws CameraException {
if (AllCamerasByName.size() == 0) throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA);
var curCam = AllCamerasByName.get(SettingsManager.GeneralSettings.curr_camera);
if (curCam == null) throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA);
return curCam;
}
private static boolean addCamera(Camera camera, String cameraName) {
if (AllCamerasByName.containsKey(cameraName)) return false;
camera.addPipeline();
AllCamerasByName.put(cameraName, camera);
return true;
}
public static void setCurrentCamera(String cameraName) throws CameraException {
if (!AllCamerasByName.containsKey(cameraName))
throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA);
SettingsManager.GeneralSettings.curr_camera = cameraName;
SettingsManager.updateCameraSetting(cameraName, getCurrentCamera().getCurrentPipelineIndex());
}
private static Camera getCamera(String cameraName) {
return AllCamerasByName.get(cameraName);
}
public static Pipeline getCurrentPipeline() throws CameraException {
return getCurrentCamera().getCurrentPipeline();
}
public static void setCurrentCamera(String cameraName) throws CameraException {
if (!AllCamerasByName.containsKey(cameraName))
throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA);
SettingsManager.GeneralSettings.curr_camera = cameraName;
SettingsManager.updateCameraSetting(cameraName, getCurrentCamera().getCurrentPipelineIndex());
}
public static void setCurrentPipeline(int pipelineNumber) throws CameraException {
if (!getCurrentCamera().getPipelines().containsKey(pipelineNumber))
throw new CameraException(CameraException.CameraExceptionType.BAD_PIPELINE);
getCurrentCamera().setCurrentPipelineIndex(pipelineNumber);
SettingsManager.updatePipelineSetting(pipelineNumber);
}
public static Camera getCurrentCamera() throws CameraException {
if (AllCamerasByName.size() == 0) throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA);
var curCam = AllCamerasByName.get(SettingsManager.GeneralSettings.curr_camera);
if (curCam == null) throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA);
return curCam;
}
public static List<String> getResolutionList() throws CameraException {
if (!SettingsManager.GeneralSettings.curr_camera.equals("")) {
List<String> list = new ArrayList<>();
for (var res : CameraManager.getCamera(SettingsManager.GeneralSettings.curr_camera).getAvailableVideoModes()) {
list.add(String.format("%s X %s at %s fps", res.width, res.height, res.fps));
}
return list;
}
throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA);
}
public static void setCurrentPipeline(int pipelineNumber) throws CameraException {
if (!getCurrentCamera().getPipelines().containsKey(pipelineNumber))
throw new CameraException(CameraException.CameraExceptionType.BAD_PIPELINE);
getCurrentCamera().setCurrentPipelineIndex(pipelineNumber);
SettingsManager.updatePipelineSetting(pipelineNumber);
}
public static Pipeline getCurrentPipeline() throws CameraException {
return getCurrentCamera().getCurrentPipeline();
}
public static List<String> getResolutionList() throws CameraException {
if (!SettingsManager.GeneralSettings.curr_camera.equals("")) {
List<String> list = new ArrayList<>();
var cam = CameraManager.getCamera(SettingsManager.GeneralSettings.curr_camera).UsbCam;
for (var res : cam.enumerateVideoModes()) {
list.add(String.format("%s X %s at %s fps", res.width, res.height, res.fps));
}
return list;
}
throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA);
}
public static void saveCameras() {
for (var entry : AllCamerasByName.entrySet()) {
try {
Gson gson = new GsonBuilder().setPrettyPrinting().registerTypeAdapter(Camera.class, new CameraSerializer()).create();
FileWriter writer = new FileWriter(Paths.get(CamConfigPath.toString(), String.format("%s.json", entry.getKey())).toString());
gson.toJson(entry.getValue(), writer);
writer.flush();
writer.close();
} catch (IOException ex) {
ex.printStackTrace();
}
}
}
public static void saveCameras() {
for (var entry : AllCamerasByName.entrySet()) {
try {
Gson gson = new GsonBuilder().setPrettyPrinting().registerTypeAdapter(Camera.class, new CameraSerializer()).create();
FileWriter writer = new FileWriter(Paths.get(CamConfigPath.toString(), String.format("%s.json", entry.getKey())).toString());
gson.toJson(entry.getValue(), writer);
writer.flush();
writer.close();
} catch (IOException ex) {
ex.printStackTrace();
}
}
}
}

350
Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java
View File

@@ -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;
}
}
}

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

@@ -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();
}
}
}
}

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

@@ -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());
}
}
}
}

8
Main/src/main/java/com/chameleonvision/web/ServerHandler.java
View File

@@ -3,6 +3,7 @@ package com.chameleonvision.web;
import com.chameleonvision.CameraException;
import com.chameleonvision.settings.SettingsManager;
import com.chameleonvision.vision.Pipeline;
import com.chameleonvision.vision.camera.Camera;
import com.chameleonvision.vision.camera.CameraManager;
import edu.wpi.cscore.VideoException;
import io.javalin.websocket.WsCloseContext;
@@ -83,9 +84,8 @@ public class ServerHandler {
String newCamera = (String) value;
System.out.printf("Changing camera to %s\n", newCamera);
CameraManager.setCurrentCamera(newCamera);
broadcastMessage(new HashMap<String, Object>(){}.put("port",CameraManager.CameraPorts.get(SettingsManager.GeneralSettings.curr_camera)));
broadcastMessage(new HashMap<String, Object>(){}.put("port", CameraManager.getCurrentCamera().getStreamPort()));
broadcastMessage(CameraManager.getCurrentCamera()); //TODO CHECK JSON FOR CAMERA CHANGE
break;
case "curr_pipeline":
String newPipeline = (String) value;
@@ -172,7 +172,7 @@ public class ServerHandler {
return map;
}
private static void sendFullSettings() {
public static void sendFullSettings() {
//General settings
Map<String, Object> fullSettings = new HashMap<>(allFieldsToMap(SettingsManager.GeneralSettings));
fullSettings.put("cameraList", CameraManager.getAllCamerasByName().keySet());
@@ -183,7 +183,7 @@ public class ServerHandler {
fullSettings.put("resolutionList", CameraManager.getResolutionList());
fullSettings.put("resolution", currentCamera.getVideoModeIndex());
fullSettings.put("FOV", currentCamera.getFOV());
fullSettings.put("port", CameraManager.CameraPorts.get(SettingsManager.GeneralSettings.curr_camera));
fullSettings.put("port", currentCamera.getStreamPort());
} catch (CameraException e) {
System.err.println("No camera found!");
//TODO: add message to ui to inform that there are no cameras