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

Replace classes with new classabstraction classes

Browse Source
This commit is contained in:
Banks Troutman
2019-11-23 11:55:20 -05:00
parent 4bcae6590f
commit 4dc86c6f25
64 changed files with 396 additions and 2066 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,28 +0,0 @@
package com.chameleonvision.vision;
import java.util.Arrays;
import java.util.List;
public class Pipeline {
public int exposure = 50;
public int brightness = 50;
public Orientation orientation = Orientation.Normal;
public List<Number> hue = Arrays.asList(50, 180);
public List<Number> saturation = Arrays.asList(50, 255);
public List<Number> value = Arrays.asList(50, 255);
public boolean erode = false;
public boolean dilate = false;
public List<Number> area = Arrays.asList(0.0, 100.0);
public List<Number> ratio = Arrays.asList(0.0, 20.0);
public List<Number> extent = Arrays.asList(0, 100);
public Number speckle = 5;
public boolean isBinary = false;
public SortMode sortMode = SortMode.Largest;
public TargetGroup targetGroup = TargetGroup.Single;
public TargetIntersection targetIntersection = TargetIntersection.Up;
public double m = 1;
public double b = 0;
public List<Number> point = Arrays.asList(0,0);
public CalibrationMode calibrationMode = CalibrationMode.None;
public String nickname;
}

123
Main/src/main/java/com/chameleonvision/vision/VisionManager.java Normal file
View File

@@ -0,0 +1,123 @@
package com.chameleonvision.vision;
import com.chameleonvision.config.CameraConfig;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.util.Platform;
import com.chameleonvision.vision.camera.CameraProcess;
import com.chameleonvision.vision.camera.USBCameraProcess;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.UsbCameraInfo;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.videoio.VideoCapture;
import java.util.*;
import java.util.stream.Collectors;
public class VisionManager {
private VisionManager() {
}
private static final LinkedHashMap<String, UsbCameraInfo> UsbCameraInfosByCameraName = new LinkedHashMap<>();
private static final LinkedList<CameraConfig> LoadedCameraConfigs = new LinkedList<>();
private static final LinkedHashMap<Integer, Pair<VisionProcess, String>> VisionProcessesByIndex = new LinkedHashMap<>();
private static VisionProcess currentUIVisionProcess;
public static boolean initializeSources() {
int suffix = 0;
for (UsbCameraInfo info : UsbCamera.enumerateUsbCameras()) {
VideoCapture cap = new VideoCapture(info.dev);
if (cap.isOpened()) {
cap.release();
String name = info.name;
while (UsbCameraInfosByCameraName.containsKey(name)) {
suffix++;
name = String.format("%s (%d)", name, suffix);
}
UsbCameraInfosByCameraName.put(name, info);
}
}
if (UsbCameraInfosByCameraName.isEmpty()) {
return false;
}
// load the config
List<CameraConfig> preliminaryConfigs = new ArrayList<>();
UsbCameraInfosByCameraName.values().forEach((cameraInfo) -> {
String truePath;
if (Platform.CurrentPlatform.isWindows()) {
truePath = cameraInfo.path;
} else {
truePath = Arrays.stream(cameraInfo.otherPaths).filter(x -> x.contains("/dev/v4l/by-path")).findFirst().orElse(cameraInfo.path);
}
preliminaryConfigs.add(new CameraConfig(truePath, cameraInfo.name));
});
LoadedCameraConfigs.addAll(ConfigManager.initializeCameraConfig(preliminaryConfigs));
// TODO: (HIGH) Load pipelines from json
// UsbCameraInfosByCameraName.forEach((cameraName, cameraInfo) -> {
// Path cameraConfigFolder = Paths.get(CamConfigPath.toString(), String.format("%s\\", cameraName));
// Path cameraConfigPath = Paths.get(cameraConfigFolder.toString(), String.format("%s.json", cameraName));
// Path cameraPipelinesPath = Paths.get(cameraConfigFolder.toString(), "pipelines.json");
// Path cameraDrivermodePath = Paths.get(cameraConfigFolder.toString(), "drivermode.json");
return true;
}
public static boolean initializeProcesses() {
for (int i = 0; i < LoadedCameraConfigs.size(); i++) {
CameraConfig config = LoadedCameraConfigs.get(i);
CameraProcess camera = new USBCameraProcess(config);
VisionProcess process = new VisionProcess(camera, config.name);
VisionProcessesByIndex.put(i, Pair.of(process, config.name));
}
currentUIVisionProcess = VisionProcessesByIndex.get(0).getLeft();
return true;
}
public static void startProcesses() {
VisionProcessesByIndex.forEach((index, processNamePair) -> {
processNamePair.getLeft().start();
});
}
public static VisionProcess getCurrentUIVisionProcess() {
return currentUIVisionProcess;
}
public static void setCurrentProcessByIndex(int processIndex) {
if (processIndex > VisionProcessesByIndex.size() - 1) {
return;
}
currentUIVisionProcess = VisionProcessesByIndex.get(processIndex).getLeft();
}
public static VisionProcess getVisionProcessByIndex(int processIndex) {
if (processIndex > VisionProcessesByIndex.size() - 1) {
return null;
}
return VisionProcessesByIndex.get(0).getLeft();
}
public static List<String> getAllCameraNicknames() {
return VisionProcessesByIndex.values().stream().map(processNamePair -> processNamePair.getLeft().getCamera().getProperties().getNickname()).collect(Collectors.toList());
}
public static void saveCameras() {
VisionProcessesByIndex.forEach((index, process) -> {
VisionProcess p = process.getLeft();
String name = process.getRight();
List<CVPipelineSettings> pipelines = p.getPipelines().stream().map(cvPipeline -> cvPipeline.settings).collect(Collectors.toList());
CVPipelineSettings driverMode = p.getDriverModeSettings();
//TODO: get camera config and serialize into folder with camera name
});
}
}

407
Main/src/main/java/com/chameleonvision/vision/VisionProcess.java Normal file
View File

@@ -0,0 +1,407 @@
package com.chameleonvision.vision;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.util.LoopingRunnable;
import com.chameleonvision.vision.camera.CameraProcess;
import com.chameleonvision.vision.camera.CameraStreamer;
import com.chameleonvision.vision.pipeline.*;
import com.chameleonvision.web.ServerHandler;
import com.google.gson.Gson;
import com.google.gson.GsonBuilder;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.networktables.EntryListenerFlags;
import edu.wpi.first.networktables.EntryNotification;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpiutil.CircularBuffer;
import org.opencv.core.Mat;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
public class VisionProcess {
private final CameraProcess cameraProcess;
private final List<CVPipeline> pipelines = new ArrayList<>();
private final CameraFrameRunnable cameraRunnable;
private final CameraStreamerRunnable streamRunnable;
private final VisionProcessRunnable visionRunnable;
public final CameraStreamer cameraStreamer;
private CVPipeline currentPipeline;
private int currentPipelineIndex = 0;
private final CVPipelineSettings driverModeSettings = new CVPipelineSettings();
private CVPipeline driverModePipeline = new DriverVisionPipeline(driverModeSettings);
// shitty stuff
private volatile Mat lastCameraFrame = new Mat();
private volatile boolean hasUnprocessedFrame = true;
private volatile CVPipelineResult lastPipelineResult;
// network table stuff
private NetworkTableEntry ntPipelineEntry;
private NetworkTableEntry ntDriverModeEntry;
private int ntDriveModeListenerID;
private int ntPipelineListenerID;
private NetworkTableEntry ntYawEntry;
private NetworkTableEntry ntPitchEntry;
private NetworkTableEntry ntAuxListEntry;
private NetworkTableEntry ntAreaEntry;
private NetworkTableEntry ntTimeStampEntry;
private NetworkTableEntry ntValidEntry;
private Gson gson = new GsonBuilder().setPrettyPrinting().create();
VisionProcess(CameraProcess cameraProcess, String name) {
this.cameraProcess = cameraProcess;
pipelines.add(new CVPipeline2d("New Pipeline"));
setPipeline(0);
// Thread to grab frames from the camera
// TODO: (HIGH) fix video modes!!!
// TODO: (HIGH) FIX FPS!!!!!!!
this.cameraRunnable = new CameraFrameRunnable(cameraProcess.getProperties().videoModes.get(0).fps);
lastPipelineResult = new DriverVisionPipeline.DriverPipelineResult(
null, cameraRunnable.getFrame(new Mat()), 0
);
// Thread to put frames on the dashboard
this.cameraStreamer = new CameraStreamer(cameraProcess, name);
this.streamRunnable = new CameraStreamerRunnable(30, cameraStreamer);
// Thread to process vision data
this.visionRunnable = new VisionProcessRunnable();
}
public void start() {
System.out.println("Starting camera thread.");
new Thread(cameraRunnable).start();
while (cameraRunnable.cameraFrame == null) {
try {
if (lastCameraFrame.cols() > 0) break;
} catch (Exception e) {
// e.printStackTrace();
}
}
System.out.println("Starting vision thread.");
new Thread(visionRunnable).start();
System.out.println("Starting stream thread.");
new Thread(streamRunnable).start();
}
/**
* Removes the old value change listeners
* calls {@link #initNT}
*
* @param newTable passed to {@link #initNT}
*/
public void resetNT(NetworkTable newTable) {
ntDriverModeEntry.removeListener(ntDriveModeListenerID);
ntPipelineEntry.removeListener(ntPipelineListenerID);
initNT(newTable);
}
private void initNT(NetworkTable newTable) {
ntPipelineEntry = newTable.getEntry("pipeline");
ntDriverModeEntry = newTable.getEntry("driver_mode");
ntPitchEntry = newTable.getEntry("pitch");
ntYawEntry = newTable.getEntry("yaw");
ntAreaEntry = newTable.getEntry("area");
ntTimeStampEntry = newTable.getEntry("timestamp");
ntValidEntry = newTable.getEntry("is_valid");
ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate);
ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate);
ntDriverModeEntry.setBoolean(false);
ntPipelineEntry.setNumber(0);
}
private void setDriverMode(EntryNotification driverModeEntryNotification) {
setDriverMode(driverModeEntryNotification.value.getBoolean());
}
public void setDriverMode(boolean driverMode) {
if (driverMode) {
setPipelineInternal(driverModePipeline);
} else {
setPipeline(currentPipelineIndex);
}
}
/**
* Method called by the nt entry listener to update the next pipeline.
* @param notification the notification
*/
private void setPipeline(EntryNotification notification) {
var wantedPipelineIndex = (int) notification.value.getDouble();
if (wantedPipelineIndex >= pipelines.size()) {
ntPipelineEntry.setNumber(currentPipelineIndex);
} else {
currentPipelineIndex = wantedPipelineIndex;
setPipeline(wantedPipelineIndex);
}
}
public void setPipeline(int pipelineIndex) {
CVPipeline newPipeline = pipelines.get(pipelineIndex);
if (newPipeline != null) {
setPipelineInternal(newPipeline);
currentPipelineIndex = pipelineIndex;
// update the configManager
if(ConfigManager.settings.currentCamera.equals(cameraProcess.getProperties().name)) {
ConfigManager.settings.currentPipeline = pipelineIndex;
HashMap<String, Object> pipeChange = new HashMap<>();
pipeChange.put("currentPipeline", pipelineIndex);
ServerHandler.broadcastMessage(pipeChange);
ServerHandler.sendFullSettings();
}
}
}
private void setPipelineInternal(CVPipeline pipeline) {
currentPipeline = pipeline;
currentPipeline.initPipeline(cameraProcess);
}
private void updateUI(CVPipelineResult data) {
if(cameraProcess.getProperties().name.equals(ConfigManager.settings.currentCamera)) {
HashMap<String, Object> WebSend = new HashMap<>();
HashMap<String, Object> point = new HashMap<>();
HashMap<String, Object> calculated = new HashMap<>();
List<Double> center = new ArrayList<>();
if (data.hasTarget) {
if(data instanceof CVPipeline2d.CVPipeline2dResult) {
CVPipeline2d.CVPipeline2dResult result = (CVPipeline2d.CVPipeline2dResult) data;
CVPipeline2d.Target2d bestTarget = result.targets.get(0);
center.add(bestTarget.rawPoint.center.x);
center.add(bestTarget.rawPoint.center.y);
calculated.put("pitch", bestTarget.pitch);
calculated.put("yaw", bestTarget.yaw);
} else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
// TODO: (2.1) 3d stuff in UI
} else {
center.add(0.0);
center.add(0.0);
calculated.put("pitch", 0);
calculated.put("yaw", 0);
}
} else {
center.add(0.0);
center.add(0.0);
calculated.put("pitch", 0);
calculated.put("yaw", 0);
}
point.put("fps", visionRunnable.fps);
point.put("calculated", calculated);
point.put("rawPoint", center);
WebSend.put("point", point);
ServerHandler.broadcastMessage(WebSend);
}
}
private void updateNetworkTableData(CVPipelineResult data) {
ntValidEntry.setBoolean(data.hasTarget);
if(data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) {
if(data instanceof CVPipeline2d.CVPipeline2dResult) {
ntTimeStampEntry.setDouble(data.processTime);
//noinspection unchecked
List<CVPipeline2d.Target2d> targets = (List<CVPipeline2d.Target2d>) data.targets;
ntPitchEntry.setDouble(targets.get(0).pitch);
ntYawEntry.setDouble(targets.get(0).yaw);
ntAreaEntry.setDouble(targets.get(0).area);
ntAuxListEntry.setString(gson.toJson(targets));
} else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
// TODO: (2.1) 3d stuff...
}
} else {
ntPitchEntry.setDouble(0.0);
ntYawEntry.setDouble(0.0);
ntAreaEntry.setDouble(0.0);
ntTimeStampEntry.setDouble(0.0);
}
}
public void setVideoMode(VideoMode newMode) {
cameraProcess.setVideoMode(newMode);
cameraStreamer.setNewVideoMode(newMode);
}
public List<CVPipeline> getPipelines() {
return pipelines;
}
public CVPipeline getCurrentPipeline() {
return currentPipeline;
}
public int getCurrentPipelineIndex() {
return currentPipelineIndex;
}
public void addPipeline() {
// TODO: (2.1) add to UI option between 2d and 3d pipeline
pipelines.add(new CVPipeline2d());
}
public void addPipeline(CVPipeline pipeline) {
pipelines.add(pipeline);
}
public CameraProcess getCamera() {
return cameraProcess;
}
public boolean getDriverMode() {
return (currentPipeline == driverModePipeline);
}
public CVPipelineSettings getDriverModeSettings() {
return driverModePipeline.settings;
}
public CVPipeline getPipelineByIndex(int pipelineIndex) {
return pipelines.get(pipelineIndex);
}
/**
* CameraFrameRunnable grabs images from the cameraProcess
* at a specified loopTime
*/
protected class CameraFrameRunnable extends LoopingRunnable {
private Mat cameraFrame;
private long timestampMicros;
private final Object frameLock = new Object();
/**
* CameraFrameRunnable grabs images from the cameraProcess
* at a specified framerate
* @param cameraFPS FPS of camera
*/
CameraFrameRunnable(int cameraFPS) {
// add 2 FPS to allow for a bit of overhead
// TODO: (low) test the effect of this
super(1000L/(cameraFPS + 2));
}
@Override
public void process() {
System.out.println("running camera grabber process");
// Grab camera frames
var camData = cameraProcess.getFrame();
if (camData.getLeft().cols() > 0) {
// System.out.println("grabbing frame");
// synchronized (frameLock) {
// cameraFrame = camData.getLeft();
// }
timestampMicros = camData.getRight();
camData.getLeft().copyTo(lastCameraFrame);
hasUnprocessedFrame = true;
}
}
public Mat getFrame(Mat dst) {
if (cameraFrame != null) {
dst = cameraFrame;
} else {
System.out.println("no frame");
}
return dst;
}
}
/**
* VisionProcessRunnable will process images as quickly as possible
*/
private class VisionProcessRunnable implements Runnable {
volatile Double fps = 0.0;
private CircularBuffer fpsAveragingBuffer = new CircularBuffer(7);
@SuppressWarnings("FieldCanBeLocal")
private CVPipelineResult result;
private Mat streamBuffer = new Mat();
@Override
public void run() {
var lastUpdateTimeNanos = System.nanoTime();
while(!Thread.interrupted()) {
System.out.println("running vision process");
while(!hasUnprocessedFrame) {
try {
Thread.sleep(3);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
lastCameraFrame.copyTo(streamBuffer); // = //cameraRunnable.getFrame(streamBuffer);
hasUnprocessedFrame = false;
if (streamBuffer.cols() > 0 && streamBuffer.rows() > 0) {
result = currentPipeline.runPipeline(streamBuffer);
lastPipelineResult = result;
updateNetworkTableData(lastPipelineResult);
updateUI(lastPipelineResult);
} else {
// System.err.println("Bad streambuffer mat");
}
var deltaTimeNanos = lastUpdateTimeNanos - System.nanoTime();
fpsAveragingBuffer.addFirst(1.0 / (deltaTimeNanos * 1E-09));
lastUpdateTimeNanos = System.nanoTime();
fps = getAverageFPS();
// TODO: (HIGH) do something with the result
}
}
public double getAverageFPS() {
var temp = 0.0;
for(int i = 0; i < 7; i++) {
temp += fpsAveragingBuffer.get(i);
}
temp /= 7.0;
return temp;
}
}
private class CameraStreamerRunnable extends LoopingRunnable {
public final CameraStreamer streamer;
private Mat streamBuffer = new Mat();
private CameraStreamerRunnable(int cameraFPS, CameraStreamer streamer) {
// add 2 FPS to allow for a bit of overhead
// TODO: (low) test the effect of this
super(1000L/(cameraFPS + 2));
this.streamer = streamer;
}
@Override
protected void process() {
System.out.println("running camera streamer");
Mat latestMat = lastPipelineResult.outputMat; //visionRunnable.result;
if (latestMat != null && latestMat.cols() > 0) {
latestMat.copyTo(streamBuffer);
streamer.runStream(streamBuffer);
// if (toStreamMat != null && toStreamMat.cols() > 0) {
// } else {
// System.out.println("fuuuuck");
// }
}
}
}
}

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

@@ -1,53 +0,0 @@
package com.chameleonvision.vision.camera;
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 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 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;
}
}
}

61
Main/src/main/java/com/chameleonvision/vision/camera/CameraDeserializer.java
View File

@@ -1,61 +0,0 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.vision.Pipeline;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.JavaType;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.type.TypeFactory;
import com.google.gson.*;
import java.lang.reflect.Type;
import java.util.ArrayList;
import java.util.List;
public class CameraDeserializer implements JsonDeserializer<USBCamera> {
@Override
public USBCamera deserialize(JsonElement jsonElement, Type type, JsonDeserializationContext context) throws JsonParseException {
try {
var jsonObj = jsonElement.getAsJsonObject();
var camFOV = jsonObj.get("FOV").getAsDouble();
var camName = jsonObj.get("name").getAsString();
var camNickname = jsonObj.get("nickname").getAsString();
var videoModeIndex = jsonObj.get("resolution").getAsInt();
// new for 2.0
var isDriverObj = jsonObj.get("isDriver");
var driverExposureObj = jsonObj.get("driverExposure");
var driverBrightnessObj = jsonObj.get("driverBrightness");
var divisorObj = jsonObj.get("streamDivisor");
// always null-check new features
boolean isDriver = isDriverObj != null && isDriverObj.getAsBoolean();
int driverExposure = driverExposureObj == null ? USBCamera.DEFAULT_EXPOSURE : driverExposureObj.getAsInt();
int driverBrightness = driverBrightnessObj == null ? USBCamera.DEFAULT_BRIGHTNESS : driverBrightnessObj.getAsInt();
StreamDivisor divisor = divisorObj == null ? StreamDivisor.NONE : StreamDivisor.values()[divisorObj.getAsInt()];
var pipelines = jsonObj.get("pipelines");
List<Pipeline> actualPipelines = new ArrayList<>();
ObjectMapper mapper = new ObjectMapper();
TypeFactory typeFactory = mapper.getTypeFactory();
JavaType arrayType = typeFactory.constructCollectionType(List.class, Pipeline.class);
try {
actualPipelines = mapper.readValue(pipelines.toString(), arrayType);
} catch (JsonProcessingException e) {
e.printStackTrace();
}
var newCamera = actualPipelines != null ? new USBCamera(camName, camFOV, actualPipelines, videoModeIndex, divisor, isDriver) : new USBCamera(camName, camFOV, videoModeIndex, divisor, isDriver);
newCamera.setNickname(camNickname != null ? camNickname : "");
newCamera.setDriverExposure(driverExposure);
newCamera.setDriverBrightness(driverBrightness);
return newCamera;
}
catch (NullPointerException e)
{
System.err.println("Error while reading json, value doesn't exist!");
System.err.println("Try to delete the camera settings in settings/cameras/YOURCAMERA.json");
e.printStackTrace();
return null;
}
}
}

25
Main/src/main/java/com/chameleonvision/vision/camera/CameraException.java
View File

@@ -1,25 +0,0 @@
package com.chameleonvision.vision.camera;
public class CameraException extends Exception {
public enum CameraExceptionType {
NO_CAMERA,
BAD_CAMERA,
BAD_PIPELINE,
BAD_SETTING;
@Override
public String toString() {
switch (this) {
case NO_CAMERA: return "No camera connected!";
case BAD_CAMERA: return "Invalid camera!";
case BAD_PIPELINE: return "Invalid pipeline!";
case BAD_SETTING: return "Invalid camera/pipeline setting!";
default: return "Unknown camera exception!";
}
}
}
public CameraException(CameraExceptionType camExceptionType) {
super(camExceptionType.toString());
}
}

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

@@ -1,164 +0,0 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.util.FileHelper;
import com.chameleonvision.settings.SettingsManager;
import com.chameleonvision.vision.Pipeline;
import com.chameleonvision.vision.process.USBCameraProcess;
import com.chameleonvision.vision.process.VisionProcess;
import com.google.gson.Gson;
import com.google.gson.GsonBuilder;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.UsbCameraInfo;
import org.opencv.videoio.VideoCapture;
import java.io.*;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.*;
import java.util.stream.Collectors;
public class CameraManager {
private static final Path CamConfigPath = Paths.get(SettingsManager.SettingsPath.toString(), "cameras");
private static LinkedHashMap<String, USBCamera> allCamerasByName = new LinkedHashMap<>();
public static HashMap<String, VisionProcess> allVisionProcessesByName = 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, USBCamera> getAllCamerasByName() {
return allCamerasByName;
}
public static List<String> getAllCameraByNickname() {
var cameras = getAllCamerasByName();
return cameras.values().stream().map(USBCamera::getNickname).collect(Collectors.toList());
}
public static boolean initializeCameras() {
if (allUsbCameraInfosByName.size() == 0) return false;
FileHelper.CheckPath(CamConfigPath);
allUsbCameraInfosByName.forEach((key, value) -> {
var camPath = Paths.get(CamConfigPath.toString(), String.format("%s.json", key));
File camJsonFile = new File(camPath.toString());
if (camJsonFile.exists() && camJsonFile.length() != 0) {
try {
Gson gson = new GsonBuilder().registerTypeAdapter(USBCamera.class, new CameraDeserializer()).create();
var camJsonFileReader = new FileReader(camPath.toString());
var gsonRead = gson.fromJson(camJsonFileReader, USBCamera.class);
allCamerasByName.put(key, gsonRead);
} catch (FileNotFoundException ex) {
ex.printStackTrace();
}
} else {
if (!addCamera(new USBCamera(key), key)) {
System.err.println("Failed to add camera! Already exists!");
}
}
});
return true;
}
public static void initializeThreads(){
allCamerasByName.forEach((name, camera) -> {
VisionProcess visionProcess = new VisionProcess(new USBCameraProcess(camera));
allVisionProcessesByName.put(name, visionProcess);
new Thread(visionProcess).start();
});
}
private static boolean addCamera(USBCamera USBCamera, String cameraName) {
if (allCamerasByName.containsKey(cameraName)) return false;
USBCamera.addPipeline();
allCamerasByName.put(cameraName, USBCamera);
return true;
}
private static USBCamera getCamera(String cameraName) {
return allCamerasByName.get(cameraName);
}
public static USBCamera getCameraByIndex(int index) {
return allCamerasByName.get( (allCamerasByName.keySet().toArray())[ index ] );
}
public static USBCamera getCurrentCamera() throws CameraException {
if (allCamerasByName.size() == 0) throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA);
var curCam = allCamerasByName.get(SettingsManager.generalSettings.currentCamera);
if (curCam == null) throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA);
return curCam;
}
public static Integer getCurrentCameraIndex() throws CameraException {
if (allCamerasByName.size() == 0) throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA);
List<String> arr = new ArrayList<>(allCamerasByName.keySet());
for (var i = 0; i < allCamerasByName.size(); i++){
if (SettingsManager.generalSettings.currentCamera.equals(arr.get(i))){
return i;
}
}
return null;
}
public static void setCurrentCamera(String cameraName) throws CameraException {
if (!allCamerasByName.containsKey(cameraName))
throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA);
SettingsManager.generalSettings.currentCamera = cameraName;
SettingsManager.updateCameraSetting(cameraName, getCurrentCamera().getCurrentPipelineIndex());
}
public static void setCurrentCamera(int cameraIndex) throws CameraException {
List<String> s = new ArrayList<String>(allCamerasByName.keySet());
setCurrentCamera(s.get(cameraIndex));
}
public static Pipeline getCurrentPipeline() throws CameraException {
return getCurrentCamera().getCurrentPipeline();
}
public static void setCurrentPipeline(int pipelineNumber) throws CameraException {
if (pipelineNumber >= getCurrentCamera().getPipelines().size()){
throw new CameraException(CameraException.CameraExceptionType.BAD_PIPELINE);
}
getCurrentCamera().setCurrentPipelineIndex(pipelineNumber);
SettingsManager.updatePipelineSetting(pipelineNumber);
}
public static VisionProcess getVisionProcessByCameraName(String cameraName) {
return allVisionProcessesByName.get(cameraName);
}
public static VisionProcess getCurrentVisionProcess() throws CameraException {
if (!SettingsManager.generalSettings.currentCamera.equals("")){
return allVisionProcessesByName.get(SettingsManager.generalSettings.currentCamera);
}
throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA);
}
public static void saveCameras() {
for (var entry : allCamerasByName.entrySet()) {
try {
Gson gson = new GsonBuilder().setPrettyPrinting().registerTypeAdapter(USBCamera.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();
}
}
}
}

33
Main/src/main/java/com/chameleonvision/vision/camera/CameraProcess.java Normal file
View File

@@ -0,0 +1,33 @@
package com.chameleonvision.vision.camera;
import edu.wpi.cscore.VideoMode;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
public interface CameraProcess {
USBCameraProperties getProperties();
/**
* Get the next camera frame
* @return a Pair of the captured image and how long it took to grab the frame (in uS)
*/
Pair<Mat, Long> getFrame();
/**
* Set the exposure of the camera
* @param exposure the new exposure to set the camera to
*/
void setExposure(int exposure);
/**
* Set the exposure of the camera
* @param brightness the new brightness to set the camera to
*/
void setBrightness(int brightness);
/**
* Set the video mode (fps and resolution) of the camera
* @param mode the wanted mode
*/
void setVideoMode(VideoMode mode);
}

24
Main/src/main/java/com/chameleonvision/vision/camera/CameraSerializer.java
View File

@@ -1,24 +0,0 @@
package com.chameleonvision.vision.camera;
import com.google.gson.*;
import java.lang.reflect.Type;
public class CameraSerializer implements JsonSerializer<USBCamera> {
@Override
public JsonElement serialize(USBCamera USBCamera, Type type, JsonSerializationContext context) {
JsonObject obj = new JsonObject();
obj.addProperty("FOV", USBCamera.getFOV());
obj.addProperty("path", USBCamera.path);
obj.addProperty("name", USBCamera.name);
obj.addProperty("nickname", USBCamera.getNickname());
obj.addProperty("streamDivisor", USBCamera.getStreamDivisor().ordinal());
var pipelines = context.serialize(USBCamera.getPipelines());
obj.add("pipelines", pipelines);
obj.addProperty("resolution", USBCamera.getVideoModeIndex());
obj.add("camVideoMode", context.serialize(USBCamera.getVideoMode()));
obj.add("isDriver",context.serialize(USBCamera.getDriverMode()));
obj.add("driverExposure",context.serialize(USBCamera.getDriverExposure()));
obj.add("driverBrightness",context.serialize(USBCamera.getDriverBrightness()));
return obj;
}
}

36
Main/src/main/java/com/chameleonvision/vision/camera/CameraStaticProperties.java Normal file
View File

@@ -0,0 +1,36 @@
package com.chameleonvision.vision.camera;
import org.apache.commons.math3.fraction.Fraction;
import org.apache.commons.math3.util.FastMath;
public class CameraStaticProperties {
public final int imageWidth;
public final int imageHeight;
public final double fov;
public final double imageArea;
public final double centerX;
public final double centerY;
public final double horizontalFocalLength;
public final double verticalFocalLength;
public CameraStaticProperties(int imageWidth, int imageHeight, double fov) {
this.imageWidth = imageWidth;
this.imageHeight = imageHeight;
this.fov = fov;
imageArea = this.imageWidth * this.imageHeight;
centerX = ((double) this.imageWidth / 2) - 0.5;
centerY = ((double) this.imageHeight / 2) - 0.5;
// pinhole model calculations
double diagonalView = FastMath.toRadians(this.fov);
Fraction aspectFraction = new Fraction(this.imageWidth, this.imageHeight);
int horizontalRatio = aspectFraction.getNumerator();
int verticalRatio = aspectFraction.getDenominator();
double diagonalAspect = FastMath.hypot(horizontalRatio, verticalRatio);
double horizontalView = FastMath.atan(FastMath.tan(diagonalView / 2) * (horizontalRatio / diagonalAspect)) * 2;
double verticalView = FastMath.atan(FastMath.tan(diagonalView / 2) * (verticalRatio / diagonalAspect)) * 2;
horizontalFocalLength = this.imageWidth / (2 * FastMath.tan(horizontalView /2));
verticalFocalLength = this.imageHeight / (2 * FastMath.tan(verticalView /2));
}
}

65
Main/src/main/java/com/chameleonvision/vision/camera/CameraStreamer.java Normal file
View File

@@ -0,0 +1,65 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.vision.enums.StreamDivisor;
import com.chameleonvision.web.ServerHandler;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.cameraserver.CameraServer;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
public class CameraStreamer {
private final CameraProcess cameraProcess;
private final String name;
private StreamDivisor divisor = StreamDivisor.NONE;
private CvSource cvSource;
private final Object streamBufferLock = new Object();
private Mat streamBuffer = new Mat();
public CameraStreamer(CameraProcess cameraProcess, String name) {
this.cameraProcess = cameraProcess;
this.name = name;
this.cvSource = CameraServer.getInstance().putVideo(name,
cameraProcess.getProperties().staticProperties.imageWidth / divisor.value,
cameraProcess.getProperties().staticProperties.imageHeight / divisor.value);
setDivisor(divisor);
}
public void setDivisor(StreamDivisor newDivisor) {
this.divisor = newDivisor;
var camValues = cameraProcess.getProperties();
var newWidth = camValues.staticProperties.imageWidth / newDivisor.value;
var newHeight = camValues.staticProperties.imageHeight / newDivisor.value;
synchronized (streamBufferLock) {
this.streamBuffer = new Mat(newWidth, newHeight, CvType.CV_8UC3);
this.cvSource = CameraServer.getInstance().putVideo(this.name,
cameraProcess.getProperties().staticProperties.imageWidth / divisor.value,
cameraProcess.getProperties().staticProperties.imageHeight / divisor.value);
}
ServerHandler.sendFullSettings();
}
public StreamDivisor getDivisor() {
return divisor;
}
public void setNewVideoMode(VideoMode newVideoMode) {
// Trick to update cvSource and streamBuffer to the new resolution
// Must change the cameraProcess resolution first
setDivisor(divisor);
}
public void runStream(Mat image) {
synchronized (streamBufferLock) {
streamBuffer = image;
}
// if (divisor.value != 1) {
// var camVal = cameraProcess.getProperties().staticProperties;
// var newWidth = camVal.imageWidth / divisor.value;
// var newHeight = camVal.imageHeight / divisor.value;
// Size newSize = new Size(newWidth, newHeight);
// Imgproc.resize(streamBuffer, streamBuffer, newSize);
// }
cvSource.putFrame(streamBuffer);
}
}

49
Main/src/main/java/com/chameleonvision/vision/camera/CameraValues.java
View File

@@ -1,49 +0,0 @@
package com.chameleonvision.vision.camera;
import org.apache.commons.math3.fraction.Fraction;
import org.apache.commons.math3.util.FastMath;
public class CameraValues {
public final int ImageWidth;
public final int ImageHeight;
public final double FOV;
public final double ImageArea;
public final double CenterX;
public final double CenterY;
private final double HorizontalFocalLength;
private final double VerticalFocalLength;
public CameraValues(USBCamera USBCamera) {
this(USBCamera.getVideoMode().width, USBCamera.getVideoMode().height, USBCamera.getFOV());
}
public CameraValues(int imageWidth, int imageHeight, double fov) {
ImageWidth = imageWidth;
ImageHeight = imageHeight;
FOV = fov;
ImageArea = ImageWidth * ImageHeight;
CenterX = ((double) ImageWidth / 2) - 0.5;
CenterY = ((double) ImageHeight / 2) - 0.5;
// pinhole model calculations
double diagonalView = FastMath.toRadians(FOV);
Fraction aspectFraction = new Fraction(ImageWidth, ImageHeight);
int horizontalRatio = aspectFraction.getNumerator();
int verticalRatio = aspectFraction.getDenominator();
double diagonalAspect = FastMath.hypot(horizontalRatio, verticalRatio);
double horizontalView = FastMath.atan(FastMath.tan(diagonalView / 2) * (horizontalRatio / diagonalAspect)) * 2;
double verticalView = FastMath.atan(FastMath.tan(diagonalView / 2) * (verticalRatio / diagonalAspect)) * 2;
HorizontalFocalLength = ImageWidth / (2 * FastMath.tan(horizontalView /2));
VerticalFocalLength = ImageHeight / (2 * FastMath.tan(verticalView /2));
}
public double CalculatePitch(double PixelY, double centerY) {
double pitch = FastMath.toDegrees(FastMath.atan((PixelY - centerY) / VerticalFocalLength));
return (pitch * -1);
}
public double CalculateYaw(double PixelX, double centerX) {
return FastMath.toDegrees(FastMath.atan((PixelX - centerX) / HorizontalFocalLength));
}
}

347
Main/src/main/java/com/chameleonvision/vision/camera/USBCamera.java
View File

@@ -1,347 +0,0 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.settings.Platform;
import com.chameleonvision.vision.Pipeline;
import com.chameleonvision.web.ServerHandler;
import edu.wpi.cscore.*;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import org.opencv.core.Mat;
import java.util.*;
import java.util.stream.Collectors;
import java.util.stream.IntStream;
public class USBCamera {
private static final double DEFAULT_FOV = 60.8;
private static final StreamDivisor DEFAULT_STREAMDIVISOR = StreamDivisor.NONE;
public static final int DEFAULT_EXPOSURE = 50;
public static final int DEFAULT_BRIGHTNESS = 50;
private static final int MINIMUM_FPS = 30;
private static final int MINIMUM_WIDTH = 320;
private static final int MINIMUM_HEIGHT = 200;
private static final int MAX_INIT_MS = 1500;
private static final List<VideoMode.PixelFormat> ALLOWED_PIXEL_FORMATS = Arrays.asList(VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG);
public final String name;
public final String path;
private String nickname;
private final UsbCamera UsbCam;
private final VideoMode[] availableVideoModes;
private final CameraServer cs = CameraServer.getInstance();
private final CvSink cvSink;
private final Object cvSourceLock = new Object();
private CvSource cvSource;
private Double FOV;
private StreamDivisor streamDivisor;
private CameraValues camVals;
private CamVideoMode camVideoMode;
private int currentPipelineIndex;
private List<Pipeline> pipelines;
//Driver mode camera settings
private int driverExposure;
private int driverBrightness;
private boolean isDriver;
public USBCamera(String cameraName) {
this(cameraName, DEFAULT_FOV);
}
public USBCamera(String cameraName, double fov) {
this(cameraName, CameraManager.allUsbCameraInfosByName.get(cameraName), fov);
}
public USBCamera(String cameraName, UsbCameraInfo usbCameraInfo, double fov) {
this(cameraName, usbCameraInfo, fov, DEFAULT_STREAMDIVISOR);
}
public USBCamera(String cameraName, UsbCameraInfo usbCamInfo, double fov, StreamDivisor divisor) {
this(cameraName, usbCamInfo, fov, new ArrayList<>(), 0, divisor, false);
}
public USBCamera(String cameraName, double fov, List<Pipeline> pipelines, int videoModeIndex, StreamDivisor divisor, boolean isDriver) {
this(cameraName, CameraManager.allUsbCameraInfosByName.get(cameraName), fov, pipelines, videoModeIndex, divisor, isDriver);
}
public USBCamera(String cameraName, double fov, int videoModeIndex, StreamDivisor divisor, boolean isDriver) {
this(cameraName, fov, new ArrayList<>(), videoModeIndex, divisor, isDriver);
}
public USBCamera(String cameraName, UsbCameraInfo usbCamInfo, double fov, List<Pipeline> pipelines, int videoModeIndex, StreamDivisor divisor, boolean isDriver) {
FOV = fov;
name = cameraName;
if (Platform.CurrentPlatform.isWindows()) {
path = usbCamInfo.path;
} else {
var truePath = Arrays.stream(usbCamInfo.otherPaths).filter(x -> x.contains("/dev/v4l/by-path")).findFirst();
path = truePath.orElse(usbCamInfo.path);
}
streamDivisor = divisor;
UsbCam = new UsbCamera(name, path);
this.pipelines = pipelines;
// set up video modes according to minimums
if (Platform.CurrentPlatform == Platform.WINDOWS_64 && !UsbCam.isConnected()) {
System.out.print("Waiting on camera... ");
long initTimeout = System.nanoTime();
while (!UsbCam.isConnected()) {
if (((System.nanoTime() - initTimeout) / 1e6) >= MAX_INIT_MS) {
break;
}
}
var initTimeMs = (System.nanoTime() - initTimeout) / 1e6;
System.out.printf("USBCameraProcess initialized in %.2fms\n", initTimeMs);
}
var trueVideoModes = UsbCam.enumerateVideoModes();
availableVideoModes = Arrays.stream(trueVideoModes).filter(v ->
v.fps >= MINIMUM_FPS && v.width >= MINIMUM_WIDTH && v.height >= MINIMUM_HEIGHT && ALLOWED_PIXEL_FORMATS.contains(v.pixelFormat)).toArray(VideoMode[]::new);
if (availableVideoModes.length == 0) {
System.err.println("USBCameraProcess not supported!");
throw new RuntimeException(new CameraException(CameraException.CameraExceptionType.BAD_CAMERA));
}
if (videoModeIndex <= availableVideoModes.length - 1) {
setCamVideoMode(videoModeIndex, false);
} else {
setCamVideoMode(0, false);
}
cvSink = cs.getVideo(UsbCam);
cvSource = cs.putVideo(name, camVals.ImageWidth / streamDivisor.value , camVals.ImageHeight / streamDivisor.value);
}
VideoMode[] getAvailableVideoModes() {
return availableVideoModes;
}
public int getStreamPort() {
var s = (MjpegServer) cs.getServer("serve_" + name);
return s.getPort();
}
public void setCamVideoMode(int videoMode, boolean updateCvSource) {
setCamVideoMode(new CamVideoMode(availableVideoModes[videoMode]), updateCvSource);
}
private void setCamVideoMode(CamVideoMode newVideoMode, boolean updateCvSource) {
var prevVideoMode = this.camVideoMode;
this.camVideoMode = newVideoMode;
// update camera values
camVals = new CameraValues(this);
if (prevVideoMode != null && !prevVideoMode.equals(newVideoMode)) { // if resolution changed
UsbCam.setVideoMode(newVideoMode.getActualPixelFormat(), newVideoMode.width, newVideoMode.height, newVideoMode.fps);
if (updateCvSource) {
updateCvSource();
}
}
}
private void updateCvSource() {
synchronized (cvSourceLock) {
var newWidth = camVideoMode.width / streamDivisor.value;
var newHeight = camVideoMode.height / streamDivisor.value;
cvSource = cs.putVideo(name, newWidth, newHeight);
}
CameraManager.getVisionProcessByCameraName(name).cameraProcess.updateFrameSize();
ServerHandler.sendFullSettings();
}
public void addPipeline() {
Pipeline p = new Pipeline();
p.nickname = "New pipeline " + pipelines.size();
addPipeline(p);
}
public void addPipeline(Pipeline p) {
this.pipelines.add(p);
}
public void deletePipeline(int index) {
pipelines.remove(index);
}
public void deletePipeline() {
deletePipeline(getCurrentPipelineIndex());
}
public Pipeline getCurrentPipeline() {
return getPipelineByIndex(currentPipelineIndex);
}
public Pipeline getPipelineByIndex(int pipelineIndex) {
return pipelines.get(pipelineIndex);
}
public int getCurrentPipelineIndex() {
return currentPipelineIndex;
}
public void setCurrentPipelineIndex(int pipelineNumber) {
if (pipelineNumber - 1 > pipelines.size()) return;
currentPipelineIndex = pipelineNumber;
}
public StreamDivisor getStreamDivisor() {
return streamDivisor;
}
public void setStreamDivisor(int divisor, boolean updateCvSource) {
streamDivisor = StreamDivisor.values()[divisor];
if (updateCvSource) {
updateCvSource();
}
}
public List<Pipeline> getPipelines() {
return pipelines;
}
public List<String> getPipelinesNickname() {
var pipelines = getPipelines();
return pipelines.stream().map(pipeline -> pipeline.nickname).collect(Collectors.toList());
}
public CamVideoMode getVideoMode() {
return camVideoMode;
}
public int getVideoModeIndex() {
return IntStream.range(0, availableVideoModes.length)
.filter(i -> camVideoMode.equals(availableVideoModes[i]))
.findFirst()
.orElse(-1);
}
public double getFOV() {
return FOV;
}
public void setFOV(Number fov) {
FOV = fov.doubleValue();
camVals = new CameraValues(this);
}
public void setDriverMode(boolean state)
{
isDriver = state;
if( isDriver ) {
setBrightness(driverBrightness); //We call setBrightness because it updates after 2 calls
setBrightness(driverBrightness); //Check it after we update to 2020 libraries
setExposure(driverExposure);
}
else{
UsbCam.setBrightness(getCurrentPipeline().brightness);
UsbCam.setBrightness(getCurrentPipeline().brightness);
try{UsbCam.setExposureManual(getCurrentPipeline().exposure);}
catch (VideoException e)
{
System.out.println("Exposure change isn't supported");
}
}
}
public boolean getDriverMode()
{
return isDriver;
}
public int getBrightness() {
return UsbCam.getBrightness();
}
public void setBrightness(int brightness) {
if (isDriver) {
driverBrightness = brightness;
UsbCam.setBrightness(brightness); // set twice to reduce timeout
}
else {
getCurrentPipeline().brightness = brightness;
}
UsbCam.setBrightness(brightness);
}
public void setExposure(int exposure) {
if (isDriver) {
driverExposure = exposure;
}
else {
getCurrentPipeline().exposure = exposure;
}
try {
UsbCam.setExposureManual(exposure);
} catch (VideoException e) {
System.err.println("USBCameraProcess Does not support exposure change");
}
}
public long grabFrame(Mat image) {
return cvSink.grabFrame(image);
}
public CameraValues getCamVals() {
return camVals;
}
public void putFrame(Mat image) {
synchronized (cvSourceLock) {
cvSource.putFrame(image);
}
}
public List<String> getResolutionList() {
return Arrays.stream(availableVideoModes)
.map(res -> String.format("%sx%s@%sFPS, %s", res.width, res.height, res.fps, res.pixelFormat.toString()))
.collect(Collectors.toList());
}
public void setNickname(String newNickname) {
//Deletes old camera nt table
NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + this.nickname).getInstance().deleteAllEntries();
nickname = newNickname;
if (CameraManager.allVisionProcessesByName.containsKey(this.name)) {
NetworkTable newNT = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + this.nickname);
CameraManager.allVisionProcessesByName.get(this.name).resetNT(newNT);
}
}
public String getNickname() {
return nickname == null ? name : nickname;
}
public void setDriverExposure(int exposure) {
driverExposure = exposure;
if (isDriver) {
setExposure(exposure);
}
}
public void setDriverBrightness(int brightness) {
driverBrightness = brightness;
if (isDriver) {
setBrightness(brightness);
}
}
public int getDriverExposure() {
return driverExposure;
}
public int getDriverBrightness() {
return driverBrightness;
}
}

67
Main/src/main/java/com/chameleonvision/vision/camera/USBCameraProcess.java Normal file
View File

@@ -0,0 +1,67 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.config.CameraConfig;
import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoException;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.cameraserver.CameraServer;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
public class USBCameraProcess implements CameraProcess {
private final UsbCamera baseCamera;
private final CvSink cvSink;
private Mat imageBuffer = new Mat();
private USBCameraProperties properties;
public USBCameraProcess(CameraConfig config) {
baseCamera = new UsbCamera(config.name, config.path);
cvSink = CameraServer.getInstance().getVideo(baseCamera);
properties = new USBCameraProperties(baseCamera, config);
setVideoMode(properties.videoModes.get(0));
}
@Override
public USBCameraProperties getProperties() {
return properties;
}
@Override
public Pair<Mat, Long> getFrame() {
Long deltaTime;
synchronized (cvSink) {
deltaTime = cvSink.grabFrame(imageBuffer) * 1000L;
}
return Pair.of(imageBuffer, deltaTime);
}
@Override
public void setExposure(int exposure) {
try {
baseCamera.setExposureManual(exposure);
} catch (VideoException e) {
System.err.println("Current camera does not support exposure change");
}
}
@Override
public void setBrightness(int brightness) {
try {
baseCamera.setBrightness(brightness);
} catch (VideoException e) {
System.err.println("Current camera does not support brightness change");
}
}
@Override
public void setVideoMode(VideoMode mode) {
try {
baseCamera.setVideoMode(mode);
properties.updateVideoMode(mode);
} catch (VideoException e) {
System.err.println("Current camera does not support resolution change");
}
}
}

101
Main/src/main/java/com/chameleonvision/vision/camera/USBCameraProperties.java Normal file
View File

@@ -0,0 +1,101 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.config.CameraConfig;
import com.chameleonvision.util.Platform;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoMode;
import org.apache.commons.math3.util.FastMath;
import java.util.Arrays;
import java.util.List;
import java.util.function.Predicate;
import java.util.stream.Collectors;
import java.util.stream.Stream;
public class USBCameraProperties {
public static final double DEFAULT_FOV = 70;
private static final int DEFAULT_EXPOSURE = 50;
private static final int DEFAULT_BRIGHTNESS = 50;
private static final int MINIMUM_FPS = 30;
private static final int MINIMUM_WIDTH = 320;
private static final int MINIMUM_HEIGHT = 200;
private static final int MAX_INIT_MS = 1500;
private static final List<VideoMode.PixelFormat> ALLOWED_PIXEL_FORMATS = Arrays.asList(VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG);
private static final Predicate<VideoMode> kMinFPSPredicate = (videoMode -> videoMode.fps >= MINIMUM_FPS);
private static final Predicate<VideoMode> kMinSizePredicate = (videoMode -> videoMode.width >= MINIMUM_WIDTH && videoMode.height >= MINIMUM_HEIGHT);
private static final Predicate<VideoMode> kPixelFormatPredicate = (videoMode -> ALLOWED_PIXEL_FORMATS.contains(videoMode.pixelFormat));
public CameraStaticProperties staticProperties;
public final String name;
public final String path;
public final List<VideoMode> videoModes;
private final UsbCamera baseCamera;
private String nickname;
public double FOV;
public final boolean hasGain;
public USBCameraProperties(UsbCamera baseCamera, CameraConfig config) {
FOV = config.fov;
name = config.name;
path = config.path;
nickname = config.nickname;
this.baseCamera = baseCamera;
// wait for camera USB init on Windows, Windows USB is slow...
if (Platform.CurrentPlatform == Platform.WINDOWS_64 && !baseCamera.isConnected()) {
System.out.print("Waiting on camera... ");
long initTimeout = System.nanoTime();
while (!baseCamera.isConnected()) {
if (((System.nanoTime() - initTimeout) / 1e6) >= MAX_INIT_MS) {
break;
}
}
var initTimeMs = (System.nanoTime() - initTimeout) / 1e6;
System.out.printf("USBCameraProcess initialized in %.2fms\n", initTimeMs);
}
// TODO: (low) find way to determine if camera is a PS3Eye
hasGain = false;
// var props = baseCamera.enumerateProperties();
// for (var prop : props) {
// var name = prop.getName();
// var min = prop.getMin();
// var max = prop.getMax();
// var _default = prop.getDefault();
// var kind = prop.getKind();
// }
videoModes = filterVideoModes(baseCamera.enumerateVideoModes());
}
public void setNickname(String nickname) {
this.nickname = nickname;
}
public String getNickname() {
return nickname;
}
private List<VideoMode> filterVideoModes(VideoMode[] videoModes) {
Predicate<VideoMode> fullPredicate = kMinFPSPredicate.and(kMinSizePredicate).and(kPixelFormatPredicate);
Stream<VideoMode> validModes = Arrays.stream(videoModes).filter(fullPredicate);
return validModes.collect(Collectors.toList());
}
public void updateVideoMode(VideoMode videoMode) {
staticProperties = new CameraStaticProperties(videoMode.width, videoMode.height, FOV);
}
public double calculatePitch(double PixelY, double centerY) {
double pitch = FastMath.toDegrees(FastMath.atan((PixelY - centerY) / staticProperties.verticalFocalLength));
return (pitch * -1);
}
public double calculateYaw(double PixelX, double centerX) {
return FastMath.toDegrees(FastMath.atan((PixelX - centerX) / staticProperties.horizontalFocalLength));
}
}

2
Main/src/main/java/com/chameleonvision/vision/CalibrationMode.java → Main/src/main/java/com/chameleonvision/vision/enums/CalibrationMode.java
View File

@@ -1,4 +1,4 @@
package com.chameleonvision.vision;
package com.chameleonvision.vision.enums;
public enum CalibrationMode {
None,Single,Dual

2
Main/src/main/java/com/chameleonvision/vision/ImageFlipMode.java → Main/src/main/java/com/chameleonvision/vision/enums/ImageFlipMode.java
View File

@@ -1,4 +1,4 @@
package com.chameleonvision.vision;
package com.chameleonvision.vision.enums;
public enum ImageFlipMode {
NONE(Integer.MIN_VALUE),

2
Main/src/main/java/com/chameleonvision/vision/ImageRotation.java → Main/src/main/java/com/chameleonvision/vision/enums/ImageRotation.java
View File

@@ -1,4 +1,4 @@
package com.chameleonvision.vision;
package com.chameleonvision.vision.enums;
import org.opencv.core.Core;

2
Main/src/main/java/com/chameleonvision/vision/Orientation.java → Main/src/main/java/com/chameleonvision/vision/enums/Orientation.java
View File

@@ -1,4 +1,4 @@
package com.chameleonvision.vision;
package com.chameleonvision.vision.enums;
public enum Orientation {
//TODO: (low) add 90 and 270 deg rotation?

2
Main/src/main/java/com/chameleonvision/vision/SortMode.java → Main/src/main/java/com/chameleonvision/vision/enums/SortMode.java
View File

@@ -1,4 +1,4 @@
package com.chameleonvision.vision;
package com.chameleonvision.vision.enums;
public enum SortMode {
Largest,Smallest,Highest,Lowest,Rightmost,Leftmost,Centermost

2
Main/src/main/java/com/chameleonvision/vision/camera/StreamDivisor.java → Main/src/main/java/com/chameleonvision/vision/enums/StreamDivisor.java
View File

@@ -1,4 +1,4 @@
package com.chameleonvision.vision.camera;
package com.chameleonvision.vision.enums;
public enum StreamDivisor {
NONE(1),

2
Main/src/main/java/com/chameleonvision/vision/TargetGroup.java → Main/src/main/java/com/chameleonvision/vision/enums/TargetGroup.java
View File

@@ -1,4 +1,4 @@
package com.chameleonvision.vision;
package com.chameleonvision.vision.enums;
public enum TargetGroup {
Single,

2
Main/src/main/java/com/chameleonvision/vision/TargetIntersection.java → Main/src/main/java/com/chameleonvision/vision/enums/TargetIntersection.java
View File

@@ -1,4 +1,4 @@
package com.chameleonvision.vision;
package com.chameleonvision.vision.enums;
public enum TargetIntersection {
None,Up,Down,Left,Right

30
Main/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline.java Normal file
View File

@@ -0,0 +1,30 @@
package com.chameleonvision.vision.pipeline;
import com.chameleonvision.vision.camera.CameraProcess;
import org.opencv.core.Mat;
/**
*
* @param <R> Pipeline result type
*/
public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings> {
protected Mat outputMat = new Mat();
CameraProcess cameraProcess;
public final S settings;
protected CVPipeline(S settings) {
this.settings = settings;
}
protected CVPipeline(String pipelineName, S settings) {
this.settings = settings;
settings.nickname = pipelineName;
}
public void initPipeline(CameraProcess camera) {
cameraProcess = camera;
cameraProcess.setExposure((int) settings.exposure);
cameraProcess.setBrightness((int) settings.brightness);
}
abstract public R runPipeline(Mat inputMat);
}

147
Main/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline2d.java Normal file
View File

@@ -0,0 +1,147 @@
package com.chameleonvision.vision.pipeline;
import com.chameleonvision.vision.camera.CameraStaticProperties;
import com.chameleonvision.vision.pipeline.pipes.*;
import com.chameleonvision.vision.enums.ImageRotation;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import java.util.List;
import static com.chameleonvision.vision.pipeline.CVPipeline2d.*;
@SuppressWarnings("WeakerAccess")
public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSettings> {
private Mat rawCameraMat = new Mat();
private Mat hsvOutputMat = new Mat();
public CVPipeline2d() {
super(new CVPipeline2dSettings());
}
public CVPipeline2d(String name) {
super(name, new CVPipeline2dSettings());
}
public CVPipeline2d(CVPipeline2dSettings settings) {
super(settings);
}
@Override
public CVPipeline2dResult runPipeline(Mat inputMat) {
if (cameraProcess == null) {
throw new RuntimeException("Pipeline was not initialized before being run!");
}
long totalProcessTimeNanos = 0;
StringBuilder procTimeStringBuilder = new StringBuilder();
CameraStaticProperties camProps = cameraProcess.getProperties().staticProperties;
rawCameraMat = inputMat;
// prepare pipes
RotateFlipPipe rotateFlipPipe = new RotateFlipPipe(ImageRotation.DEG_0, settings.flipMode);
BlurPipe blurPipe = new BlurPipe(5);
ErodeDilatePipe erodeDilatePipe = new ErodeDilatePipe(settings.erode, settings.dilate, 7);
Scalar hsvLower = new Scalar(settings.hue.get(0).intValue(), settings.saturation.get(0).intValue(), settings.value.get(0).intValue());
Scalar hsvUpper = new Scalar(settings.hue.get(1).intValue(), settings.saturation.get(1).intValue(), settings.value.get(1).intValue());
HsvPipe hsvPipe = new HsvPipe(hsvLower, hsvUpper);
FindContoursPipe findContoursPipe = new FindContoursPipe();
FilterContoursPipe filterContoursPipe = new FilterContoursPipe(settings.area, settings.ratio, settings.extent, camProps);
SpeckleRejectPipe speckleRejectPipe = new SpeckleRejectPipe(settings.speckle.doubleValue());
GroupContoursPipe groupContoursPipe = new GroupContoursPipe(settings.targetGroup, settings.targetIntersection);
SortContoursPipe sortContoursPipe = new SortContoursPipe(settings.sortMode, camProps);
Collect2dTargetsPipe collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode, settings.point,
settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps);
OutputMatPipe outputMatPipe = new OutputMatPipe(settings.isBinary);
Draw2dContoursPipe.Draw2dContoursSettings draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings();
draw2dContoursSettings.showCentroid = false;
draw2dContoursSettings.showCrosshair = true;
draw2dContoursSettings.boxOutlineSize = 2;
draw2dContoursSettings.showRotatedBox = true;
draw2dContoursSettings.showMaximumBox = true;
Draw2dContoursPipe draw2dContoursPipe = new Draw2dContoursPipe(draw2dContoursSettings, camProps);
// run pipes
Pair<Mat, Long> rotateFlipResult = rotateFlipPipe.run(inputMat);
totalProcessTimeNanos += rotateFlipResult.getRight();
procTimeStringBuilder.append(String.format("RotateFlip: %.2fms, ", rotateFlipResult.getRight() / 1000.0));
Pair<Mat, Long> blurResult = blurPipe.run(rotateFlipResult.getLeft());
totalProcessTimeNanos += blurResult.getRight();
procTimeStringBuilder.append(String.format("Blur: %.2fms, ", blurResult.getRight() / 1000.0));
Pair<Mat, Long> erodeDilateResult = erodeDilatePipe.run(blurResult.getLeft());
totalProcessTimeNanos += erodeDilateResult.getRight();
procTimeStringBuilder.append(String.format("ErodeDilate: %.2fms, ", erodeDilateResult.getRight() / 1000.0));
Pair<Mat, Long> hsvResult = hsvPipe.run(erodeDilateResult.getLeft());
totalProcessTimeNanos += hsvResult.getRight();
Imgproc.cvtColor(hsvResult.getLeft(), hsvOutputMat, Imgproc.COLOR_GRAY2BGR, 3);
procTimeStringBuilder.append(String.format("HSV: %.2fms, ", hsvResult.getRight() / 1000.0));
Pair<List<MatOfPoint>, Long> findContoursResult = findContoursPipe.run(hsvResult.getLeft());
totalProcessTimeNanos += findContoursResult.getRight();
procTimeStringBuilder.append(String.format("FindContours: %.2fms, ", findContoursResult.getRight() / 1000.0));
Pair<List<MatOfPoint>, Long> filterContoursResult = filterContoursPipe.run(findContoursResult.getLeft());
totalProcessTimeNanos += filterContoursResult.getRight();
procTimeStringBuilder.append(String.format("FilterContours: %.2fms, ", filterContoursResult.getRight() / 1000.0));
Pair<List<MatOfPoint>, Long> speckleRejectResult = speckleRejectPipe.run(filterContoursResult.getLeft());
totalProcessTimeNanos += speckleRejectResult.getRight();
procTimeStringBuilder.append(String.format("SpeckleReject: %.2fms, ", speckleRejectResult.getRight() / 1000.0));
Pair<List<RotatedRect>, Long> groupContoursResult = groupContoursPipe.run(speckleRejectResult.getLeft());
totalProcessTimeNanos += groupContoursResult.getRight();
procTimeStringBuilder.append(String.format("GroupContours: %.2fms, ", groupContoursResult.getRight() / 1000.0));
Pair<List<RotatedRect>, Long> sortContoursResult = sortContoursPipe.run(groupContoursResult.getLeft());
totalProcessTimeNanos += sortContoursResult.getRight();
procTimeStringBuilder.append(String.format("SortContours: %.2fms, ", sortContoursResult.getRight() / 1000.0));
Pair<List<Target2d>, Long> collect2dTargetsResult = collect2dTargetsPipe.run(sortContoursResult.getLeft());
totalProcessTimeNanos += collect2dTargetsResult.getRight();
procTimeStringBuilder.append(String.format("SortContours: %.2fms, ", sortContoursResult.getRight() / 1000.0));
// takes pair of (Mat of original camera image, Mat of HSV thresholded image)
Pair<Mat, Long> outputMatResult = outputMatPipe.run(Pair.of(rawCameraMat, hsvOutputMat));
totalProcessTimeNanos += outputMatResult.getRight();
procTimeStringBuilder.append(String.format("OutputMat: %.2fms, ", outputMatResult.getRight() / 1000.0));
// takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
Pair<Mat, Long> draw2dContoursResult = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft()));
totalProcessTimeNanos += draw2dContoursResult.getRight();
procTimeStringBuilder.append(String.format("Draw2dContours: %.2fms, ", draw2dContoursResult.getRight() / 1000.0));
System.out.println(procTimeStringBuilder.toString());
System.out.printf("Pipeline ran in %.3fms\n", totalProcessTimeNanos / 1000.0);
return new CVPipeline2dResult(collect2dTargetsResult.getLeft(), draw2dContoursResult.getLeft(), totalProcessTimeNanos / 1000);
}
public static class CVPipeline2dResult extends CVPipelineResult<Target2d> {
public CVPipeline2dResult(List<Target2d> targets, Mat outputMat, long processTime) {
super(targets, outputMat, processTime);
}
}
public static class Target2d {
public double calibratedX = 0.0;
public double calibratedY = 0.0;
public double pitch = 0.0;
public double yaw = 0.0;
public double area = 0.0;
public RotatedRect rawPoint;
}
}

29
Main/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline2dSettings.java Normal file
View File

@@ -0,0 +1,29 @@
package com.chameleonvision.vision.pipeline;
import com.chameleonvision.vision.enums.CalibrationMode;
import com.chameleonvision.vision.enums.SortMode;
import com.chameleonvision.vision.enums.TargetGroup;
import com.chameleonvision.vision.enums.TargetIntersection;
import java.util.Arrays;
import java.util.List;
public class CVPipeline2dSettings extends CVPipelineSettings {
public List<Number> hue = Arrays.asList(50, 180);
public List<Number> saturation = Arrays.asList(50, 255);
public List<Number> value = Arrays.asList(50, 255);
public boolean erode = false;
public boolean dilate = false;
public List<Number> area = Arrays.asList(0.0, 100.0);
public List<Number> ratio = Arrays.asList(0.0, 20.0);
public List<Number> extent = Arrays.asList(0, 100);
public Number speckle = 5;
public boolean isBinary = false;
public SortMode sortMode = SortMode.Largest;
public TargetGroup targetGroup = TargetGroup.Single;
public TargetIntersection targetIntersection = TargetIntersection.Up;
public List<Number> point = Arrays.asList(0, 0);
public CalibrationMode calibrationMode = CalibrationMode.None;
public double dualTargetCalibrationM = 1;
public double dualTargetCalibrationB = 0;
}

31
Main/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline3d.java Normal file
View File

@@ -0,0 +1,31 @@
package com.chameleonvision.vision.pipeline;
import org.opencv.core.Mat;
import java.util.List;
import static com.chameleonvision.vision.pipeline.CVPipeline3d.*;
public class CVPipeline3d extends CVPipeline<CVPipeline3dResult, CVPipeline3dSettings> {
protected CVPipeline3d(CVPipeline3dSettings settings) {
super(settings);
}
@Override
public CVPipeline3dResult runPipeline(Mat inputMat) {
return null;
}
public static class CVPipeline3dResult extends CVPipelineResult<Target3d> {
public CVPipeline3dResult(List<Target3d> targets, Mat outputMat, long processTime) {
super(targets, outputMat, processTime);
}
}
public static class Target3d {
// TODO: (2.1) Define 3d-specific target data
}
}

5
Main/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline3dSettings.java Normal file
View File

@@ -0,0 +1,5 @@
package com.chameleonvision.vision.pipeline;
public class CVPipeline3dSettings extends CVPipeline2dSettings {
// TODO: (2.1) define 3d-specific pipeline settings
}

19
Main/src/main/java/com/chameleonvision/vision/pipeline/CVPipelineResult.java Normal file
View File

@@ -0,0 +1,19 @@
package com.chameleonvision.vision.pipeline;
import org.opencv.core.Mat;
import java.util.List;
public abstract class CVPipelineResult<T> {
public final List<T> targets;
public final boolean hasTarget;
public final Mat outputMat;
public final long processTime;
public CVPipelineResult(List<T> targets, Mat outputMat, long processTime) {
this.targets = targets;
hasTarget = targets != null && !targets.isEmpty();
this.outputMat = outputMat;
this.processTime = processTime;
}
}

11
Main/src/main/java/com/chameleonvision/vision/pipeline/CVPipelineSettings.java Normal file
View File

@@ -0,0 +1,11 @@
package com.chameleonvision.vision.pipeline;
import com.chameleonvision.vision.enums.ImageFlipMode;
@SuppressWarnings("ALL")
public class CVPipelineSettings {
public ImageFlipMode flipMode = ImageFlipMode.NONE;
public String nickname = "New Pipeline";
public double exposure = 50.0;
public double brightness = 50.0;
}

38
Main/src/main/java/com/chameleonvision/vision/pipeline/DriverVisionPipeline.java Normal file
View File

@@ -0,0 +1,38 @@
package com.chameleonvision.vision.pipeline;
import com.chameleonvision.vision.pipeline.pipes.Draw2dContoursPipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import java.util.List;
import static com.chameleonvision.vision.pipeline.DriverVisionPipeline.DriverPipelineResult;
public class DriverVisionPipeline extends CVPipeline<DriverPipelineResult, CVPipelineSettings> {
public DriverVisionPipeline(CVPipelineSettings settings) {
super(settings);
}
@Override
public DriverPipelineResult runPipeline(Mat inputMat) {
outputMat = inputMat;
var camProps = cameraProcess.getProperties().staticProperties;
Draw2dContoursPipe.Draw2dContoursSettings draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings();
draw2dContoursSettings.showCrosshair = true;
Draw2dContoursPipe draw2dContoursPipe = new Draw2dContoursPipe(draw2dContoursSettings, camProps);
outputMat = draw2dContoursPipe.run(Pair.of(outputMat, null)).getLeft();
return new DriverPipelineResult(null, inputMat, 0);
}
public static class DriverPipelineResult extends CVPipelineResult<Void> {
public DriverPipelineResult(List<Void> targets, Mat outputMat, long processTime) {
super(targets, outputMat, processTime);
}
}
}

31
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/BlurPipe.java Normal file
View File

@@ -0,0 +1,31 @@
package com.chameleonvision.vision.pipeline.pipes;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
public class BlurPipe implements Pipe<Mat, Mat> {
private final int blurSize;
private Mat outputMat = new Mat();
public BlurPipe(int blurSize) {
this.blurSize = blurSize;
}
@Override
public Pair<Mat, Long> run(Mat input) {
long processStartNanos = System.nanoTime();
if (blurSize > 0) {
Imgproc.blur(outputMat, outputMat, new Size(blurSize, blurSize));
}
long processTime = processStartNanos - System.nanoTime();
Pair<Mat, Long> output = Pair.of(outputMat, processTime);
outputMat.release();
return output;
}
}

71
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/Collect2dTargetsPipe.java Normal file
View File

@@ -0,0 +1,71 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CameraStaticProperties;
import com.chameleonvision.vision.pipeline.CVPipeline2d;
import com.chameleonvision.vision.enums.CalibrationMode;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.RotatedRect;
import java.util.ArrayList;
import java.util.List;
public class Collect2dTargetsPipe implements Pipe<List<RotatedRect>, List<CVPipeline2d.Target2d>> {
private final CalibrationMode calibrationMode;
private final CameraStaticProperties camProps;
private final List<Number> calibrationPoint;
private final double calibrationM, calibrationB;
private List<CVPipeline2d.Target2d> targets = new ArrayList<>();
public Collect2dTargetsPipe(CalibrationMode calibrationMode, List<Number> calibrationPoint, double calibrationM, double calibrationB, CameraStaticProperties camProps) {
this.calibrationMode = calibrationMode;
this.camProps = camProps;
this.calibrationPoint = calibrationPoint;
this.calibrationM = calibrationM;
this.calibrationB = calibrationB;
}
@Override
public Pair<List<CVPipeline2d.Target2d>, Long> run(List<RotatedRect> input) {
long processStartNanos = System.nanoTime();
input.forEach(r -> {
CVPipeline2d.Target2d t = new CVPipeline2d.Target2d();
t.rawPoint = r;
switch (calibrationMode) {
case None:
t.calibratedX = camProps.centerX;
t.calibratedY = camProps.centerY;
break;
case Single:
t.calibratedX = calibrationPoint.get(0).doubleValue();
t.calibratedY = calibrationPoint.get(1).doubleValue();
break;
case Dual:
t.calibratedX = (r.center.y - calibrationB) / calibrationM;
t.calibratedY = (r.center.x * calibrationM) + calibrationB;
break;
}
t.pitch = calculatePitch(r.center.y, t.calibratedY);
t.yaw = calculateYaw(r.center.x, t.calibratedX);
t.area = r.size.area();
targets.add(t);
});
long processTime = processStartNanos - System.nanoTime();
return Pair.of(targets, processTime);
}
private double calculatePitch(double pixelY, double centerY) {
double pitch = FastMath.toDegrees(FastMath.atan((pixelY - centerY) / camProps.verticalFocalLength));
return (pitch * -1);
}
private double calculateYaw(double pixelX, double centerX) {
return FastMath.toDegrees(FastMath.atan((pixelX - centerX) / camProps.horizontalFocalLength));
}
}

85
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/Draw2dContoursPipe.java Normal file
View File

@@ -0,0 +1,85 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CameraStaticProperties;
import com.chameleonvision.util.Helpers;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Point;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import java.awt.*;
import java.util.ArrayList;
import java.util.List;
public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Mat> {
private final Draw2dContoursSettings settings;
private final CameraStaticProperties camProps;
private Mat outputMat = new Mat();
public Draw2dContoursPipe(Draw2dContoursSettings settings, CameraStaticProperties camProps) {
this.settings = settings;
this.camProps = camProps;
}
@Override
public Pair<Mat, Long> run(Pair<Mat, List<RotatedRect>> input) {
long processStartNanos = System.nanoTime();
outputMat = input.getLeft();
if (input.getRight() != null && !input.getRight().isEmpty()) {
for (RotatedRect r : input.getRight()) {
if (r == null) continue;
List<MatOfPoint> drawnContour = new ArrayList<>();
Point[] vertices = new Point[4];
r.points(vertices);
MatOfPoint contour = new MatOfPoint(vertices);
drawnContour.add(contour);
if (settings.showCentroid) {
Imgproc.circle(outputMat, r.center, 3, Helpers.colorToScalar(settings.centroidColor));
}
if (settings.showRotatedBox) {
Imgproc.drawContours(outputMat, drawnContour, 0, Helpers.colorToScalar(settings.rotatedBoxColor), settings.boxOutlineSize);
}
if (settings.showMaximumBox) {
Rect box = Imgproc.boundingRect(contour);
Imgproc.rectangle(outputMat, new Point(box.x, box.y), new Point((box.x + box.width), (box.y + box.height)), Helpers.colorToScalar(settings.maximumBoxColor), settings.boxOutlineSize);
}
}
}
if (settings.showCrosshair) {
Point xMax = new Point(camProps.centerX + 10, camProps.centerY);
Point xMin = new Point(camProps.centerX - 10, camProps.centerY);
Point yMax = new Point(camProps.centerX, camProps.centerY + 10);
Point yMin = new Point(camProps.centerX, camProps.centerY - 10);
if (outputMat != null && outputMat.cols() > 0) {
Imgproc.line(outputMat, xMax, xMin, Helpers.colorToScalar(settings.crosshairColor), 2);
Imgproc.line(outputMat, yMax, yMin, Helpers.colorToScalar(settings.crosshairColor), 2);
}
}
long processTime = processStartNanos - System.nanoTime();
Pair<Mat, Long> output = Pair.of(outputMat, processTime);
outputMat.release();
return output;
}
public static class Draw2dContoursSettings {
public boolean showCentroid = false;
public boolean showCrosshair = false;
public int boxOutlineSize = 0;
public boolean showRotatedBox = false;
public boolean showMaximumBox = false;
public Color centroidColor = Color.GREEN;
public Color crosshairColor = Color.GREEN;
public Color rotatedBoxColor = Color.BLUE;
public Color maximumBoxColor = Color.RED;
}
}

38
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/ErodeDilatePipe.java Normal file
View File

@@ -0,0 +1,38 @@
package com.chameleonvision.vision.pipeline.pipes;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
public class ErodeDilatePipe implements Pipe<Mat, Mat> {
private final boolean erode, dilate;
private final Mat kernel;
private Mat outputMat = new Mat();
public ErodeDilatePipe(boolean erode, boolean dilate, int kernelSize) {
this.erode = erode;
this.dilate = dilate;
kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(kernelSize, kernelSize));
}
@Override
public Pair<Mat, Long> run(Mat input) {
long processStartNanos = System.nanoTime();
if (erode) {
Imgproc.erode(outputMat, outputMat, kernel);
}
if (dilate) {
Imgproc.erode(outputMat, outputMat, kernel);
}
long processTime = processStartNanos - System.nanoTime();
Pair<Mat, Long> output = Pair.of(outputMat, processTime);
outputMat.release();
return output;
}
}

62
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/FilterContoursPipe.java Normal file
View File

@@ -0,0 +1,62 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CameraStaticProperties;
import com.chameleonvision.util.MathHandler;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Rect;
import org.opencv.imgproc.Imgproc;
import java.util.ArrayList;
import java.util.List;
public class FilterContoursPipe implements Pipe<List<MatOfPoint>, List<MatOfPoint>> {
private final List<Number> area, ratio, extent;
private final CameraStaticProperties camProps;
private List<MatOfPoint> filteredContours = new ArrayList<>();
public FilterContoursPipe(List<Number> area, List<Number> ratio, List<Number> extent, CameraStaticProperties camProps) {
this.area = area;
this.ratio = ratio;
this.extent = extent;
this.camProps = camProps;
}
@Override
public Pair<List<MatOfPoint>, Long> run(List<MatOfPoint> input) {
long processStartNanos = System.nanoTime();
for (MatOfPoint Contour : input) {
try {
double contourArea = Imgproc.contourArea(Contour);
double AreaRatio = (contourArea / camProps.imageArea) * 100;
double minArea = (MathHandler.sigmoid(area.get(0)));
double maxArea = (MathHandler.sigmoid(area.get(1)));
if (AreaRatio < minArea || AreaRatio > maxArea) {
continue;
}
var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray()));
double minExtent = (extent.get(0).doubleValue() * rect.size.area()) / 100;
double maxExtent = (extent.get(1).doubleValue() * rect.size.area()) / 100;
if (contourArea <= minExtent || contourArea >= maxExtent) {
continue;
}
Rect bb = Imgproc.boundingRect(Contour);
double aspectRatio = ((double)bb.width / bb.height);
if (aspectRatio < ratio.get(0).doubleValue() || aspectRatio > ratio.get(1).doubleValue()) {
continue;
}
filteredContours.add(Contour);
} catch (Exception e) {
System.err.println("Error while filtering contours");
e.printStackTrace();
}
}
long processTime = processStartNanos - System.nanoTime();
return Pair.of(filteredContours, processTime);
}
}

26
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/FindContoursPipe.java Normal file
View File

@@ -0,0 +1,26 @@
package com.chameleonvision.vision.pipeline.pipes;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.imgproc.Imgproc;
import java.util.ArrayList;
import java.util.List;
public class FindContoursPipe implements Pipe<Mat, List<MatOfPoint>> {
private List<MatOfPoint> foundContours = new ArrayList<>();
public FindContoursPipe() {}
@Override
public Pair<List<MatOfPoint>, Long> run(Mat input) {
long processStartNanos = System.nanoTime();
Imgproc.findContours(input, foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1);
long processTime = processStartNanos - System.nanoTime();
return Pair.of(foundContours, processTime);
}
}

154
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java Normal file
View File

@@ -0,0 +1,154 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.util.MathHandler;
import com.chameleonvision.vision.enums.TargetGroup;
import com.chameleonvision.vision.enums.TargetIntersection;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import org.opencv.imgproc.Moments;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;
public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRect>> {
private static final Comparator<MatOfPoint> sortByMomentsX =
Comparator.comparingDouble(GroupContoursPipe::calcMomentsX);
private final TargetGroup group;
private final TargetIntersection intersection;
private List<RotatedRect> groupedContours = new ArrayList<>();
private MatOfPoint2f intersectMatA = new MatOfPoint2f();
private MatOfPoint2f intersectMatB = new MatOfPoint2f();
public GroupContoursPipe(TargetGroup group, TargetIntersection intersection) {
this.group = group;
this.intersection = intersection;
}
@Override
public Pair<List<RotatedRect>, Long> run(List<MatOfPoint> input) {
long processStartNanos = System.nanoTime();
List<MatOfPoint> sorted = new ArrayList<>(input);
sorted.sort(sortByMomentsX);
Collections.reverse(sorted);
switch (group) {
case Single: {
input.forEach(c -> {
MatOfPoint2f contour = new MatOfPoint2f();
contour.fromArray(c.toArray());
if (contour.cols() != 0 && contour.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contour);
groupedContours.add(rect);
}
});
break;
}
case Dual: {
for (var i = 0; i < input.size(); i++) {
List<Point> finalContourList = new ArrayList<>(input.get(i).toList());
try {
MatOfPoint firstContour = input.get(i);
MatOfPoint secondContour = input.get(i + 1);
if (isIntersecting(firstContour, secondContour)) {
finalContourList.addAll(secondContour.toList());
} else {
finalContourList.clear();
continue;
}
firstContour.release();
secondContour.release();
MatOfPoint2f contour = new MatOfPoint2f();
contour.fromList(finalContourList);
if (contour.cols() != 0 && contour.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contour);
groupedContours.add(rect);
}
} catch (IndexOutOfBoundsException e) {
System.err.println("GroupContours: WTF");
finalContourList.clear();
}
}
break;
}
}
long processTime = processStartNanos - System.nanoTime();
return Pair.of(groupedContours, processTime);
}
private static double calcMomentsX(MatOfPoint c) {
Moments m = Imgproc.moments(c);
return (m.get_m10() / m.get_m00());
}
private boolean isIntersecting(MatOfPoint contourOne, MatOfPoint contourTwo) {
if (intersection.equals(TargetIntersection.None)) {
return true;
}
try {
intersectMatA.fromArray(contourOne.toArray());
intersectMatB.fromArray(contourTwo.toArray());
RotatedRect a = Imgproc.fitEllipse(intersectMatA);
RotatedRect b = Imgproc.fitEllipse(intersectMatB);
double mA = MathHandler.toSlope(a.angle);
double mB = MathHandler.toSlope(b.angle);
double x0A = a.center.x;
double y0A = a.center.y;
double x0B = b.center.x;
double y0B = b.center.y;
double intersectionX = ((mA * x0A) - y0A - (mB * x0B) + y0B) / (mA - mB);
double intersectionY = (mA * (intersectionX - x0A)) + y0A;
double massX = (x0A + x0B) / 2;
double massY = (y0A + y0B) / 2;
switch (intersection) {
case Up: {
if (intersectionY < massY) {
if (mA > 0 && mB < 0) {
return true;
}
}
break;
}
case Down: {
if (intersectionY > massY) {
if (mA < 0 && mB > 0) {
return true;
}
}
break;
}
case Left: {
if (intersectionX < massX) {
return true;
}
break;
}
case Right: {
if (intersectionX > massX) {
return true;
}
break;
}
}
return false;
} catch (Exception e) {
return false;
}
}
}

34
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/HsvPipe.java Normal file
View File

@@ -0,0 +1,34 @@
package com.chameleonvision.vision.pipeline.pipes;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
public class HsvPipe implements Pipe<Mat, Mat> {
private final Scalar hsvLower, hsvUpper;
private Mat outputMat = new Mat();
public HsvPipe(Scalar hsvLower, Scalar hsvUpper) {
this.hsvLower = hsvLower;
this.hsvUpper = hsvUpper;
}
@Override
public Pair<Mat, Long> run(Mat input) {
long processStartNanos = System.nanoTime();
Imgproc.cvtColor(input, outputMat, Imgproc.COLOR_RGB2HSV, 3);
Core.inRange(outputMat, hsvLower, hsvUpper, outputMat);
long processTime = processStartNanos - System.nanoTime();
Pair<Mat, Long> output = Pair.of(outputMat, processTime);
outputMat.release();
return output;
}
}

27
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/OutputMatPipe.java Normal file
View File

@@ -0,0 +1,27 @@
package com.chameleonvision.vision.pipeline.pipes;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
public class OutputMatPipe implements Pipe<Pair<Mat, Mat>, Mat> {
private boolean showThresholded;
private Mat outputMat = new Mat();
public OutputMatPipe(boolean showThresholded) {
this.showThresholded = showThresholded;
}
@Override
public Pair<Mat, Long> run(Pair<Mat, Mat> input) {
long processStartNanos = System.nanoTime();
outputMat = showThresholded ? input.getRight() : input.getLeft();
long processTime = processStartNanos - System.nanoTime();
Pair<Mat, Long> output = Pair.of(outputMat, processTime);
outputMat.release();
return output;
}
}

13
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/Pipe.java Normal file
View File

@@ -0,0 +1,13 @@
package com.chameleonvision.vision.pipeline.pipes;
import org.apache.commons.lang3.tuple.Pair;
public interface Pipe<I, O> {
/**
*
* @param input Input object for pipe
* @return Returns a Pair containing the process time in Nanoseconds,
* and the output object
*/
Pair<O, Long> run(I input);
}

33
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/RotateFlipPipe.java Normal file
View File

@@ -0,0 +1,33 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.enums.ImageFlipMode;
import com.chameleonvision.vision.enums.ImageRotation;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Core;
import org.opencv.core.Mat;
public class RotateFlipPipe implements Pipe<Mat, Mat> {
private final ImageRotation rotation;
private final ImageFlipMode flip;
private Mat outputMat = new Mat();
public RotateFlipPipe(ImageRotation rotation, ImageFlipMode flip) {
this.rotation = rotation;
this.flip = flip;
}
@Override
public Pair<Mat, Long> run(Mat input) {
long processStartNanos = System.nanoTime();
Core.flip(input, outputMat, flip.value);
Core.rotate(outputMat, outputMat, rotation.value);
long processTime = processStartNanos - System.nanoTime();
Pair<Mat, Long> output = Pair.of(outputMat, processTime);
outputMat.release();
return output;
}
}

74
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/SortContoursPipe.java Normal file
View File

@@ -0,0 +1,74 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CameraStaticProperties;
import com.chameleonvision.vision.enums.SortMode;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.RotatedRect;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.List;
public class SortContoursPipe implements Pipe<List<RotatedRect>, List<RotatedRect>> {
private final Comparator<RotatedRect> SortByCentermostComparator = Comparator.comparingDouble(this::calcCenterDistance);
private static final Comparator<RotatedRect> SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.size.area(), rect1.size.area());
private static final Comparator<RotatedRect> SortBySmallestComparator = SortByLargestComparator.reversed();
private static final Comparator<RotatedRect> SortByHighestComparator = (rect1, rect2) -> Double.compare(rect2.center.y, rect1.center.y);
private static final Comparator<RotatedRect> SortByLowestComparator = SortByHighestComparator.reversed();
private static final Comparator<RotatedRect> SortByLeftmostComparator = Comparator.comparingDouble(rect -> rect.center.x);
private static final Comparator<RotatedRect> SortByRightmostComparator = SortByLeftmostComparator.reversed();
private final SortMode sort;
private final CameraStaticProperties camProps;
private List<RotatedRect> sortedContours = new ArrayList<>();
public SortContoursPipe(SortMode sort, CameraStaticProperties camProps) {
this.sort = sort;
this.camProps = camProps;
}
@Override
public Pair<List<RotatedRect>, Long> run(List<RotatedRect> input) {
long processStartNanos = System.nanoTime();
switch (sort) {
case Largest:
input.sort(SortByLargestComparator);
break;
case Smallest:
input.sort(SortBySmallestComparator);
break;
case Highest:
input.sort(SortByHighestComparator);
break;
case Lowest:
input.sort(SortByLowestComparator);
break;
case Leftmost:
input.sort(SortByLeftmostComparator);
break;
case Rightmost:
input.sort(SortByRightmostComparator);
break;
case Centermost:
input.sort(SortByCentermostComparator);
break;
default:
break;
}
long processTime = processStartNanos - System.nanoTime();
return Pair.of(sortedContours, processTime);
}
private double calcCenterDistance(RotatedRect rect) {
return FastMath.sqrt(FastMath.pow(camProps.centerX - rect.center.x, 2) + FastMath.pow(camProps.centerY - rect.center.y, 2));
}
}

43
Main/src/main/java/com/chameleonvision/vision/pipeline/pipes/SpeckleRejectPipe.java Normal file
View File

@@ -0,0 +1,43 @@
package com.chameleonvision.vision.pipeline.pipes;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.MatOfPoint;
import org.opencv.imgproc.Imgproc;
import java.util.ArrayList;
import java.util.List;
public class SpeckleRejectPipe implements Pipe<List<MatOfPoint>, List<MatOfPoint>> {
private final double minPercentOfAvg;
private List<MatOfPoint> despeckledContours = new ArrayList<>();
public SpeckleRejectPipe(double minPercentOfAvg) {
this.minPercentOfAvg = minPercentOfAvg;
}
@Override
public Pair<List<MatOfPoint>, Long> run(List<MatOfPoint> input) {
long processStartNanos = System.nanoTime();
double averageArea = 0.0;
for (MatOfPoint c : input) {
averageArea += Imgproc.contourArea(c);
}
averageArea /= input.size();
double minAllowedArea = minPercentOfAvg / 100.0 * averageArea;
for (MatOfPoint c : input) {
if (Imgproc.contourArea(c) >= minAllowedArea) {
despeckledContours.add(c);
}
}
long processTime = processStartNanos - System.nanoTime();
return Pair.of(despeckledContours, processTime);
}
}

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

@@ -1,9 +0,0 @@
package com.chameleonvision.vision.process;
import com.chameleonvision.vision.Pipeline;
import com.chameleonvision.vision.camera.CameraValues;
import org.opencv.core.Mat;
public interface CVProcess {
PipelineResult runPipeline(Pipeline currentPipeline, Mat inputImage, Mat outputImage, CameraValues cameraValues, boolean shouldFlip, boolean driverMode);
}

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

@@ -1,30 +0,0 @@
package com.chameleonvision.vision.process;
import com.chameleonvision.vision.Pipeline;
import com.chameleonvision.vision.camera.CamVideoMode;
import com.chameleonvision.vision.camera.CameraValues;
import org.opencv.core.Mat;
import java.util.List;
public interface CameraProcess extends Runnable {
long getLatestFrame(Mat outputFrame);
void updateFrame(Mat inputFrame);
void updateFrameSize();
String getCamName();
CameraValues getCamVals();
boolean getDriverMode();
void setDriverMode(boolean isDriverMode);
List<Pipeline> getPipelines();
Pipeline getCurrentPipeline();
int getCurrentPipelineIndex();
void setExposure(int exposure);
void setBrightness(int brightness);
CamVideoMode getVideoMode();
String getNickname();
void setCurrentPipelineIndex(int ntPipelineIndex);
}

13
Main/src/main/java/com/chameleonvision/vision/process/PipelineResult.java
View File

@@ -1,13 +0,0 @@
package com.chameleonvision.vision.process;
import org.opencv.core.RotatedRect;
public class PipelineResult {
public boolean IsValid = false;
public double CalibratedX = 0.0;
public double CalibratedY = 0.0;
public double Pitch = 0.0;
public double Yaw = 0.0;
public double Area = 0.0;
RotatedRect RawPoint;
}

345
Main/src/main/java/com/chameleonvision/vision/process/StandardCVProcess.java
View File

@@ -1,345 +0,0 @@
package com.chameleonvision.vision.process;
import com.chameleonvision.vision.*;
import com.chameleonvision.vision.camera.CameraValues;
import com.chameleonvision.util.MathHandler;
import org.apache.commons.math3.util.FastMath;
import org.jetbrains.annotations.NotNull;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import org.opencv.imgproc.Moments;
import java.util.*;
@SuppressWarnings("WeakerAccess")
public class StandardCVProcess implements CVProcess {
private final CameraValues cameraValues;
private Mat kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
private Size blur = new Size(3, 3);
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<MatOfPoint> speckleRejectedContours = new ArrayList<>();
private Comparator<MatOfPoint> sortByMomentsX = Comparator.comparingDouble(this::calcMomentsX);
private List<RotatedRect> finalCountours = new ArrayList<>();
private MatOfPoint2f intersectMatA = new MatOfPoint2f();
private MatOfPoint2f intersectMatB = new MatOfPoint2f();
StandardCVProcess(CameraValues cameraValues) {
this.cameraValues = cameraValues;
}
private Mat cameraInputMat = new Mat();
private Mat hsvThreshMat = new Mat();
private Mat streamOutputMat = new Mat();
private List<MatOfPoint> foundContours_ = new ArrayList<>();
private List<MatOfPoint> filteredContours_ = new ArrayList<>();
private List<MatOfPoint> deSpeckledContours_ = new ArrayList<>();
private List<RotatedRect> groupedContours_ = new ArrayList<>();
private static final Scalar contourRectColor = new Scalar(255, 0, 0);
private static final Scalar BoxRectColor = new Scalar(0, 0, 233);
private void drawContour(Mat inputMat, RotatedRect contourRect) {
if (contourRect == null) return;
List<MatOfPoint> drawnContour = new ArrayList<>();
Point[] vertices = new Point[4];
contourRect.points(vertices);
MatOfPoint contour = new MatOfPoint(vertices);
drawnContour.add(contour);
Rect box = Imgproc.boundingRect(contour);
Imgproc.drawContours(inputMat, drawnContour, 0, contourRectColor, 3);
Imgproc.circle(inputMat, contourRect.center, 3, contourRectColor);
Imgproc.rectangle(inputMat, new Point(box.x, box.y), new Point((box.x + box.width), (box.y + box.height)), BoxRectColor, 2);
}
public PipelineResult runPipeline(Pipeline currentPipeline, Mat inputImage, Mat outputImage, CameraValues cameraValues, boolean shouldFlip, boolean driverMode) {
var pipelineResult = new PipelineResult();
if (currentPipeline == null) {
return pipelineResult;
}
// flip the image
if (shouldFlip) {
Core.flip(inputImage, inputImage, -1);
}
// if we're in driver mode don't do anything, and return a blank result
if (driverMode) {
inputImage.copyTo(outputImage);
return pipelineResult;
}
foundContours_.clear();
filteredContours_.clear();
deSpeckledContours_.clear();
groupedContours_.clear();
// HSV threshold the image
Scalar hsvLower = new Scalar(currentPipeline.hue.get(0).intValue(), currentPipeline.saturation.get(0).intValue(), currentPipeline.value.get(0).intValue());
Scalar hsvUpper = new Scalar(currentPipeline.hue.get(1).intValue(), currentPipeline.saturation.get(1).intValue(), currentPipeline.value.get(1).intValue());
hsvThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate);
// Make sure we're BFR
if (currentPipeline.isBinary) {
Imgproc.cvtColor(hsvThreshMat, outputImage, Imgproc.COLOR_GRAY2BGR, 3);
} else {
inputImage.copyTo(outputImage);
}
// search for contours
foundContours_ = findContours(hsvThreshMat);
if (foundContours_.size() < 1) {
return pipelineResult;
}
// filter contours by area, ratio and extent
filteredContours_ = filterContours(foundContours_, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent);
if (filteredContours_.size() < 1) {
return pipelineResult;
}
// reject "speckle" contours
deSpeckledContours_ = rejectSpeckles(filteredContours_, currentPipeline.speckle.doubleValue());
if (deSpeckledContours_.size() < 1) {
return pipelineResult;
}
// group targets
groupedContours_ = groupTargets(deSpeckledContours_, currentPipeline.targetIntersection, currentPipeline.targetGroup);
if (groupedContours_.size() < 1) {
return pipelineResult;
}
// sort targets down to our final target
var finalRect = sortTargetsToOne(groupedContours_, currentPipeline.sortMode);
pipelineResult.RawPoint = finalRect;
pipelineResult.IsValid = true;
switch (currentPipeline.calibrationMode) {
case None:
///use the center of the USBCameraProcess to find the pitch and yaw difference
pipelineResult.CalibratedX = cameraValues.CenterX;
pipelineResult.CalibratedY = cameraValues.CenterY;
break;
case Single:
// use the static point as a calibration method instead of the center
pipelineResult.CalibratedX = currentPipeline.point.get(0).doubleValue();
pipelineResult.CalibratedY = currentPipeline.point.get(1).doubleValue();
break;
case Dual:
// use the calculated line to find the difference in length between the point and the line
pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.b) / currentPipeline.m;
pipelineResult.CalibratedY = (finalRect.center.x * currentPipeline.m) + currentPipeline.b;
break;
}
pipelineResult.Pitch = cameraValues.CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY);
pipelineResult.Yaw = cameraValues.CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX);
pipelineResult.Area = finalRect.size.area();
drawContour(outputImage, finalRect);
return pipelineResult;
}
void hsvThreshold(Mat srcImage, Mat dst, @NotNull Scalar hsvLower, @NotNull Scalar hsvUpper, boolean shouldErode, boolean shouldDilate) {
Imgproc.cvtColor(srcImage, hsvImage, Imgproc.COLOR_RGB2HSV, 3);
Imgproc.blur(hsvImage, hsvImage, blur);
Core.inRange(hsvImage, hsvLower, hsvUpper, dst);
if (shouldErode) {
Imgproc.erode(dst, dst, kernel);
}
if (shouldDilate) {
Imgproc.dilate(dst, dst, kernel);
}
hsvImage.release();
}
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;
}
List<MatOfPoint> filterContours(List<MatOfPoint> inputContours, List<Number> area, List<Number> ratio, List<Number> extent) {
for (MatOfPoint Contour : inputContours) {
try {
double contourArea = Imgproc.contourArea(Contour);
double AreaRatio = (contourArea / cameraValues.ImageArea) * 100;
double minArea = (MathHandler.sigmoid(area.get(0)));
double maxArea = (MathHandler.sigmoid(area.get(1)));
if (AreaRatio < minArea || AreaRatio > maxArea) {
continue;
}
var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray()));
var targetFullness = contourArea;
double minExtent = (double) (extent.get(0).doubleValue() * rect.size.area()) / 100;
double maxExtent = (double) (extent.get(1).doubleValue() * rect.size.area()) / 100;
if (targetFullness <= minExtent || contourArea >= maxExtent) {
continue;
}
Rect bb = Imgproc.boundingRect(Contour);
double aspectRatio = (bb.width / bb.height);
if (aspectRatio < ratio.get(0).doubleValue() || aspectRatio > ratio.get(1).doubleValue()) {
continue;
}
filteredContours.add(Contour);
} catch (Exception e) {
System.err.println("Error while filtering contours");
e.printStackTrace();
}
}
return filteredContours;
}
List<MatOfPoint> rejectSpeckles(List<MatOfPoint> inputContours, Double minimumPercentOfAverage) {
double averageArea = 0.0;
for (MatOfPoint c : inputContours) {
averageArea += Imgproc.contourArea(c);
}
averageArea /= inputContours.size();
var minimumAllowableArea = minimumPercentOfAverage / 100.0 * averageArea;
speckleRejectedContours.clear();
for (MatOfPoint c : inputContours) {
if (Imgproc.contourArea(c) >= minimumAllowableArea) speckleRejectedContours.add(c);
}
return speckleRejectedContours;
}
private double calcDistance(RotatedRect rect) {
return FastMath.sqrt(FastMath.pow(cameraValues.CenterX - rect.center.x, 2) + FastMath.pow(cameraValues.CenterY - rect.center.y, 2));
}
private double calcMomentsX(MatOfPoint c) {
Moments m = Imgproc.moments(c);
return (m.get_m10() / m.get_m00());
}
RotatedRect sortTargetsToOne(List<RotatedRect> inputRects, SortMode 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
}
}
List<RotatedRect> groupTargets(List<MatOfPoint> inputContours, TargetIntersection intersectionPoint, TargetGroup targetGroup) {
finalCountours.clear();
inputContours.sort(sortByMomentsX);
Collections.reverse(inputContours);
if (targetGroup.equals(TargetGroup.Dual)) {
for (var i = 0; i < inputContours.size(); i++) {
List<Point> FinalContourList = new ArrayList<>(inputContours.get(i).toList());
try {
MatOfPoint firstContour = inputContours.get(i);
MatOfPoint secondContour = inputContours.get(i + 1);
if (isIntersecting(firstContour, secondContour, intersectionPoint)) {
FinalContourList.addAll(secondContour.toList());
} else {
FinalContourList.clear();
continue;
}
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();
}
}
} else if (targetGroup.equals(TargetGroup.Single)) {
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 boolean isIntersecting(MatOfPoint ContourOne, MatOfPoint ContourTwo, TargetIntersection intersectionPoint) {
if (intersectionPoint.equals(TargetIntersection.None)) {
return true;
}
try {
intersectMatA.fromArray(ContourOne.toArray());
intersectMatB.fromArray(ContourTwo.toArray());
RotatedRect a = Imgproc.fitEllipse(intersectMatA);
RotatedRect b = Imgproc.fitEllipse(intersectMatB);
double mA = MathHandler.toSlope(a.angle);
double mB = MathHandler.toSlope(b.angle);
double x0A = a.center.x;
double y0A = a.center.y;
double x0B = b.center.x;
double y0B = b.center.y;
double intersectionX = ((mA * x0A) - y0A - (mB * x0B) + y0B) / (mA - mB);
double intersectionY = (mA * (intersectionX - x0A)) + y0A;
double massX = (x0A + x0B) / 2;
double massY = (y0A + y0B) / 2;
switch (intersectionPoint) {
case Up: {
if (intersectionY < massY) {
if (mA > 0 && mB < 0) {
return true;
}
}
break;
}
case Down: {
if (intersectionY > massY) {
if (mA < 0 && mB > 0) {
return true;
}
}
break;
}
case Left: {
if (intersectionX < massX) {
return true;
}
break;
}
case Right: {
if (intersectionX > massX) {
return true;
}
break;
}
}
return false;
} catch (Exception e) {
return false;
}
}
}

144
Main/src/main/java/com/chameleonvision/vision/process/USBCameraProcess.java
View File

@@ -1,144 +0,0 @@
package com.chameleonvision.vision.process;
import com.chameleonvision.vision.Pipeline;
import com.chameleonvision.vision.camera.CamVideoMode;
import com.chameleonvision.vision.camera.CameraValues;
import com.chameleonvision.vision.camera.USBCamera;
import com.chameleonvision.vision.camera.StreamDivisor;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import java.util.List;
public class USBCameraProcess implements CameraProcess {
private final USBCamera usbCamera;
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 StreamDivisor divisor;
public USBCameraProcess(USBCamera usbCamera) {
this.usbCamera = usbCamera;
maxFPS = usbCamera.getVideoMode().fps;
updateFrameSize();
}
public void updateFrameSize() {
var camVals = usbCamera.getCamVals();
divisor = usbCamera.getStreamDivisor();
var newWidth = camVals.ImageWidth / divisor.value;
var newHeight = camVals.ImageHeight / divisor.value;
synchronized (inputFrameLock) {
inputFrame = new Mat(newWidth, newHeight, CvType.CV_8UC3);
}
synchronized (outputFrameLock) {
outputFrame = new Mat(newWidth, newHeight, CvType.CV_8UC3);
}
}
public void updateFrame(Mat inputFrame) {
synchronized (inputFrameLock) {
inputFrame.copyTo(this.inputFrame);
}
}
public long getLatestFrame(Mat outputFrame) {
synchronized (outputFrameLock) {
this.outputFrame.copyTo(outputFrame);
return timestamp;
}
}
@Override
public void run() {
while (!Thread.interrupted()) {
synchronized (outputFrameLock) {
timestamp = usbCamera.grabFrame(outputFrame);
}
synchronized (inputFrameLock) {
if (divisor.value != 1) {
var camVals = usbCamera.getCamVals();
var newWidth = camVals.ImageWidth / divisor.value;
var newHeight = camVals.ImageHeight / divisor.value;
Size newSize = new Size(newWidth, newHeight);
Imgproc.resize(inputFrame, inputFrame, newSize);
}
usbCamera.putFrame(inputFrame);
}
var msToWait = (long) 1000 / maxFPS;
try {
Thread.sleep(msToWait);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}
// USBCameraProcess stuff
@Override
public String getCamName() {
return usbCamera.name;
}
@Override
public CameraValues getCamVals() {
return usbCamera.getCamVals();
}
@Override
public boolean getDriverMode() {
return usbCamera.getDriverMode();
}
@Override
public void setDriverMode(boolean isDriverMode) {
usbCamera.setDriverMode(isDriverMode);
}
@Override
public List<Pipeline> getPipelines() {
return usbCamera.getPipelines();
}
@Override
public Pipeline getCurrentPipeline() {
return usbCamera.getCurrentPipeline();
}
@Override
public int getCurrentPipelineIndex() {
return usbCamera.getCurrentPipelineIndex();
}
@Override
public void setExposure(int exposure) {
usbCamera.setExposure(exposure);
}
@Override
public void setBrightness(int brightness) {
usbCamera.setBrightness(brightness);
}
@Override
public CamVideoMode getVideoMode() {
return usbCamera.getVideoMode();
}
@Override
public String getNickname() {
return usbCamera.getNickname();
}
@Override
public void setCurrentPipelineIndex(int wantedIndex) {
usbCamera.setCurrentPipelineIndex(wantedIndex);
}
}

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

@@ -1,269 +0,0 @@
package com.chameleonvision.vision.process;
import com.chameleonvision.classabstraction.pipeline.CVPipeline2d;
import com.chameleonvision.classabstraction.pipeline.CVPipeline2dSettings;
import com.chameleonvision.classabstraction.pipeline.DriverVisionPipeline;
import com.chameleonvision.settings.SettingsManager;
import com.chameleonvision.vision.Pipeline;
import com.chameleonvision.web.ServerHandler;
import edu.wpi.cscore.VideoException;
import edu.wpi.first.networktables.*;
import org.opencv.core.*;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import static com.chameleonvision.classabstraction.pipeline.CVPipeline2d.*;
public class VisionProcess implements Runnable {
private final String cameraName;
public final CameraProcess cameraProcess;
// NetworkTables
public NetworkTableEntry ntPipelineEntry;
public NetworkTableEntry ntDriverModeEntry;
private int ntDriveModeListenerID;
private int ntPipelineListenerID;
private NetworkTableEntry ntYawEntry;
private NetworkTableEntry ntPitchEntry;
private NetworkTableEntry ntDistanceEntry;
private NetworkTableEntry ntTimeStampEntry;
private NetworkTableEntry ntValidEntry;
// chameleon specific
private Pipeline currentPipeline;
private CVProcess cvProcess;
private CVPipeline2d cvPipeline2d;
private DriverVisionPipeline driverVisionPipeline;
// pipeline process items
// private List<MatOfPoint> foundContours = new ArrayList<>();
// private List<MatOfPoint> filteredContours = new ArrayList<>();
// private List<MatOfPoint> deSpeckledContours = new ArrayList<>();
// private List<RotatedRect> groupedContours = new ArrayList<>();
private Mat cameraInputMat = new Mat();
private Mat hsvThreshMat = new Mat();
private Mat streamOutputMat = new Mat();
private long timeStamp = 0;
public VisionProcess(CameraProcess cameraProcess) {
cvProcess = new StandardCVProcess(cameraProcess.getCamVals());
this.cameraProcess = cameraProcess; // new USBCameraProcess(cameraProcess);
this.cameraName = cameraProcess.getCamName();
initNT(NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraProcess.getNickname()));
}
private void driverModeListener(EntryNotification entryNotification) {
cameraProcess.setDriverMode(entryNotification.value.getBoolean());
}
private void pipelineListener(EntryNotification entryNotification) {
var ntPipelineIndex = (int) entryNotification.value.getDouble();
if (ntPipelineIndex >= cameraProcess.getPipelines().size()) {
ntPipelineEntry.setNumber(cameraProcess.getCurrentPipelineIndex());
} else {
var pipeline = cameraProcess.getCurrentPipeline();
cameraProcess.setCurrentPipelineIndex(ntPipelineIndex);
try {
cameraProcess.setExposure(pipeline.exposure);
} catch (VideoException e) {
System.err.println(e.toString());
}
cameraProcess.setBrightness(pipeline.brightness);
if (SettingsManager.generalSettings.currentCamera.equals(cameraName)) {
SettingsManager.generalSettings.currentPipeline = ntPipelineIndex;
HashMap<String, Object> pipeChange = new HashMap<>();
pipeChange.put("currentPipeline", ntPipelineIndex);
ServerHandler.broadcastMessage(pipeChange);
ServerHandler.sendFullSettings();
}
}
}
private void updateNetworkTables(PipelineResult pipelineResult) {
if (pipelineResult.IsValid) {
ntValidEntry.setBoolean(true);
ntYawEntry.setNumber(pipelineResult.Yaw);
ntPitchEntry.setNumber(pipelineResult.Pitch);
ntDistanceEntry.setNumber(pipelineResult.Area);
ntTimeStampEntry.setNumber(timeStamp);
NetworkTableInstance.getDefault().flush();
} else {
ntYawEntry.setNumber(0.0);
ntPitchEntry.setNumber(0.0);
ntDistanceEntry.setNumber(0.0);
ntTimeStampEntry.setNumber(timeStamp);
ntValidEntry.setBoolean(false);
}
}
private CVPipeline2dSettings pipelineTo2dSettings(Pipeline pipeline) {
CVPipeline2dSettings settings = new CVPipeline2dSettings();
settings.hue = pipeline.hue;
settings.saturation = pipeline.saturation;
settings.value = pipeline.value;
settings.erode = pipeline.erode;
settings.dilate = pipeline.dilate;
settings.area = pipeline.area;
settings.ratio = pipeline.ratio;
settings.extent = pipeline.extent;
settings.speckle = pipeline.speckle;
settings.isBinary = pipeline.isBinary;
settings.sortMode = pipeline.sortMode;
settings.targetGroup = pipeline.targetGroup;
settings.targetIntersection = pipeline.targetIntersection;
settings.point = pipeline.point;
settings.calibrationMode = pipeline.calibrationMode;
settings.nickname = pipeline.nickname;
settings.exposure = pipeline.exposure;
settings.brightness = pipeline.brightness;
settings.dualTargetCalibrationM = pipeline.m;
settings.dualTargetCalibrationB = pipeline.b;
return settings;
}
private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) {
if (cvPipeline2d == null) {
cvPipeline2d = new CVPipeline2d(pipelineTo2dSettings(currentPipeline));
}
CVPipeline2dResult result = cvPipeline2d.runPipeline(inputImage);
result.outputMat.copyTo(outputImage);
PipelineResult pipeResult = new PipelineResult();
pipeResult.IsValid = result.hasTarget;
if (!pipeResult.IsValid) {
pipeResult.CalibratedX = 0;
pipeResult.CalibratedY = 0;
pipeResult.Pitch = 0;
pipeResult.Yaw = 0;
pipeResult.Area = 0;
} else {
Target2d t = result.targets.get(0);
pipeResult.CalibratedX = t.calibratedX;
pipeResult.CalibratedY = t.calibratedY;
pipeResult.Pitch = t.pitch;
pipeResult.Yaw = t.yaw;
pipeResult.Area = t.area;
}
return pipeResult;
}
@Override
public void run() {
// processing time tracking
long startTime;
long fpsLastTime = 0;
double processTimeMs;
double fps = 0;
double uiFps = 0;
int maxFps = cameraProcess.getVideoMode().fps;
new Thread(cameraProcess).start();
long lastFrameEndNanosec = 0;
while (!Thread.interrupted()) {
startTime = System.nanoTime();
if ((startTime - lastFrameEndNanosec) * 1e-6 >= 1000.0 / (maxFps + 3)) { // 3 additional fps to allow for overhead
// foundContours.clear();
// filteredContours.clear();
// groupedContours.clear();
// deSpeckledContours.clear();
// update FPS for ui only every 0.5 seconds
if ((startTime - fpsLastTime) * 1e-6 >= 500) {
if (fps >= maxFps) {
uiFps = maxFps;
} else {
uiFps = fps;
}
fpsLastTime = System.nanoTime();
}
currentPipeline = cameraProcess.getCurrentPipeline();
// start fps counter right before grabbing input frame
timeStamp = cameraProcess.getLatestFrame(cameraInputMat);
if (cameraInputMat.cols() == 0 && cameraInputMat.rows() == 0) {
continue;
}
// get vision data
var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat);
updateNetworkTables(pipelineResult);
if (cameraName.equals(SettingsManager.generalSettings.currentCamera)) {
HashMap<String, Object> WebSend = new HashMap<>();
HashMap<String, Object> point = new HashMap<>();
HashMap<String, Object> calculated = new HashMap<>();
List<Double> center = new ArrayList<>();
if (pipelineResult.IsValid) {
center.add(pipelineResult.RawPoint.center.x);
center.add(pipelineResult.RawPoint.center.y);
calculated.put("pitch", pipelineResult.Pitch);
calculated.put("yaw", pipelineResult.Yaw);
} else {
center.add(0.0);
center.add(0.0);
calculated.put("pitch", 0);
calculated.put("yaw", 0);
}
point.put("fps", uiFps);
point.put("calculated", calculated);
point.put("rawPoint", center);
WebSend.put("point", point);
ServerHandler.broadcastMessage(WebSend);
}
cameraProcess.updateFrame(streamOutputMat);
cameraInputMat.release();
hsvThreshMat.release();
// calculate FPS
lastFrameEndNanosec = System.nanoTime();
processTimeMs = (lastFrameEndNanosec - startTime) * 1e-6;
fps = 1000 / processTimeMs;
//please dont enable if you are not debugging
// System.out.printf("%s - Process time: %-5.2fms, FPS: %-5.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n", cameraName, processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size());
}
}
}
/**
* Removes the old value change listeners
* calls {@link #initNT}
*
* @param newTable passed to {@link #initNT}
*/
public void resetNT(NetworkTable newTable) {
ntDriverModeEntry.removeListener(ntDriveModeListenerID);
ntPipelineEntry.removeListener(ntPipelineListenerID);
initNT(newTable);
}
/**
* Rebases the writing location for the vision process - pipeline output
*
* @param newTable the new writing location
*/
private void initNT(NetworkTable newTable) {
ntPipelineEntry = newTable.getEntry("pipeline");
ntDriverModeEntry = newTable.getEntry("driver_mode");
ntPitchEntry = newTable.getEntry("pitch");
ntYawEntry = newTable.getEntry("yaw");
ntDistanceEntry = newTable.getEntry("distance");
ntTimeStampEntry = newTable.getEntry("timestamp");
ntValidEntry = newTable.getEntry("is_valid");
ntDriveModeListenerID = ntDriverModeEntry.addListener(this::driverModeListener, EntryListenerFlags.kUpdate);
ntPipelineListenerID = ntPipelineEntry.addListener(this::pipelineListener, EntryListenerFlags.kUpdate);
ntDriverModeEntry.setBoolean(false);
ntPipelineEntry.setNumber(cameraProcess.getCurrentPipelineIndex());
}
}