mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-21 01:01:41 +00:00
Replace classes with new classabstraction classes
This commit is contained in:
Banks Troutman
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -1,14 +0,0 @@
|
||||
package com.chameleonvision.vision.camera;
|
||||
|
||||
public enum StreamDivisor {
|
||||
NONE(1),
|
||||
HALF(2),
|
||||
QUARTER(4),
|
||||
SIXTH(6);
|
||||
|
||||
public final Integer value;
|
||||
|
||||
StreamDivisor(int value) {
|
||||
this.value = value;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user