mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Full rewrite of camera system
This commit is contained in:
Banks Troutman
@@ -0,0 +1,25 @@
|
||||
package com.chameleonvision.vision.camera;
|
||||
|
||||
import edu.wpi.cscore.VideoMode;
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
132
Main/src/main/java/com/chameleonvision/vision/camera/Camera.java
Normal file
132
Main/src/main/java/com/chameleonvision/vision/camera/Camera.java
Normal file
@@ -0,0 +1,132 @@
|
||||
package com.chameleonvision.vision.camera;
|
||||
|
||||
import com.chameleonvision.vision.Pipeline;
|
||||
import edu.wpi.cscore.UsbCamera;
|
||||
import edu.wpi.cscore.UsbCameraInfo;
|
||||
import edu.wpi.cscore.VideoMode;
|
||||
|
||||
import java.util.HashMap;
|
||||
import java.util.stream.IntStream;
|
||||
|
||||
public class Camera {
|
||||
|
||||
private static double defaultFOV = 60.8;
|
||||
|
||||
public final String name;
|
||||
public final String path;
|
||||
|
||||
public final UsbCamera UsbCam;
|
||||
private final UsbCameraInfo UsbCamInfo;
|
||||
private final VideoMode[] availableVideoModes;
|
||||
|
||||
private double FOV;
|
||||
|
||||
private CameraValues camVals;
|
||||
private CamVideoMode camVideoMode;
|
||||
|
||||
private int currentPipelineIndex;
|
||||
private HashMap<Integer, Pipeline> pipelines = new HashMap<>();
|
||||
|
||||
public Camera(String cameraName) {
|
||||
this(cameraName, defaultFOV);
|
||||
}
|
||||
|
||||
public Camera(UsbCameraInfo usbCamInfo) {
|
||||
this(usbCamInfo, defaultFOV);
|
||||
}
|
||||
|
||||
public Camera(String cameraName, double fov) {
|
||||
this(CameraManager.AllUsbCameraInfosByName.get(cameraName), fov);
|
||||
}
|
||||
|
||||
public Camera(UsbCameraInfo usbCamInfo, double fov) {
|
||||
UsbCamInfo = usbCamInfo;
|
||||
FOV = fov;
|
||||
name = usbCamInfo.name;
|
||||
path = usbCamInfo.path;
|
||||
|
||||
UsbCam = new UsbCamera(name, path);
|
||||
|
||||
// set up video mode
|
||||
availableVideoModes = UsbCam.enumerateVideoModes();
|
||||
setCamVideoMode(new CamVideoMode(availableVideoModes[0]));
|
||||
}
|
||||
|
||||
public void setCamVideoMode(int videoMode) {
|
||||
setCamVideoMode(UsbCam.enumerateVideoModes()[videoMode]);
|
||||
}
|
||||
|
||||
private void setCamVideoMode(VideoMode videoMode) {
|
||||
setCamVideoMode(new CamVideoMode(videoMode));
|
||||
}
|
||||
|
||||
private void setCamVideoMode(CamVideoMode camVideoMode) {
|
||||
this.camVideoMode = camVideoMode;
|
||||
UsbCam.setPixelFormat(camVideoMode.getActualPixelFormat());
|
||||
UsbCam.setFPS(camVideoMode.fps);
|
||||
UsbCam.setResolution(camVideoMode.width, camVideoMode.height);
|
||||
camVals = new CameraValues(this);
|
||||
// TODO: Automatically restart CameraProcess when resolution changes (not FPS)
|
||||
}
|
||||
|
||||
public void addPipeline() {
|
||||
addPipeline(pipelines.size());
|
||||
}
|
||||
|
||||
private void addPipeline(int pipelineNumber) {
|
||||
if (pipelines.containsKey(pipelineNumber)) return;
|
||||
pipelines.put(pipelineNumber, new Pipeline());
|
||||
}
|
||||
|
||||
public void setCurrentPipelineIndex(int pipelineNumber) {
|
||||
if (pipelineNumber - 1 > pipelines.size()) return;
|
||||
currentPipelineIndex = pipelineNumber;
|
||||
}
|
||||
|
||||
public Pipeline getCurrentPipeline() {
|
||||
return pipelines.get(currentPipelineIndex);
|
||||
}
|
||||
|
||||
public int getCurrentPipelineIndex() {
|
||||
return currentPipelineIndex;
|
||||
}
|
||||
|
||||
public HashMap<Integer, Pipeline> getPipelines() {
|
||||
return pipelines;
|
||||
}
|
||||
|
||||
public CamVideoMode getVideoMode() {
|
||||
return camVideoMode;
|
||||
}
|
||||
|
||||
public int getVideoModeIndex() {
|
||||
return IntStream.range(0, availableVideoModes.length)
|
||||
.filter(i -> camVideoMode.isEqualToVideoMode(availableVideoModes[i]))
|
||||
.findFirst()
|
||||
.orElse(-1);
|
||||
}
|
||||
|
||||
public void setFOV(double fov) {
|
||||
FOV = fov;
|
||||
camVals = new CameraValues(this);
|
||||
}
|
||||
|
||||
public double getFOV() {
|
||||
return FOV;
|
||||
}
|
||||
|
||||
public void setBrightness(int brightness) {
|
||||
getCurrentPipeline().brightness = brightness;
|
||||
UsbCam.setBrightness(brightness);
|
||||
}
|
||||
|
||||
public int getBrightness() {
|
||||
return getCurrentPipeline().brightness;
|
||||
}
|
||||
|
||||
public void setExposure(int exposure) {
|
||||
getCurrentPipeline().exposure = exposure;
|
||||
UsbCam.setExposureManual(exposure);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,133 @@
|
||||
package com.chameleonvision.vision.camera;
|
||||
|
||||
import com.chameleonvision.FileHelper;
|
||||
import com.chameleonvision.CameraException;
|
||||
import com.chameleonvision.settings.SettingsManager;
|
||||
import com.chameleonvision.vision.Pipeline;
|
||||
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.FileNotFoundException;
|
||||
import java.io.FileReader;
|
||||
import java.io.FileWriter;
|
||||
import java.io.IOException;
|
||||
import java.nio.file.Files;
|
||||
import java.nio.file.Path;
|
||||
import java.nio.file.Paths;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
import java.util.List;
|
||||
|
||||
public class CameraManager {
|
||||
|
||||
public static final Path CamConfigPath = Paths.get(SettingsManager.SettingsPath.toString(), "Cams");
|
||||
|
||||
// TODO: Fix suffix for camera
|
||||
// TODO: throw a camera Exeption if no camera is connected
|
||||
static HashMap<String, UsbCameraInfo> AllUsbCameraInfosByName = new HashMap<>() {{
|
||||
var suffix = 0;
|
||||
for (var info : UsbCamera.enumerateUsbCameras()) {
|
||||
var cap = new VideoCapture(info.name);
|
||||
if (cap.isOpened()) {
|
||||
cap.release();
|
||||
var name = String.format("%s(%s)", info.name, suffix);
|
||||
put(name, info);
|
||||
suffix++;
|
||||
}
|
||||
}
|
||||
}};
|
||||
|
||||
private static HashMap<String, Camera> AllCamerasByName = new HashMap<>();
|
||||
|
||||
private static String currentCameraName;
|
||||
|
||||
public static HashMap<String, Camera> getAllCamerasByName() { return AllCamerasByName; }
|
||||
|
||||
public static void initializeCameras() {
|
||||
FileHelper.CheckPath(CamConfigPath);
|
||||
for (var entry : AllUsbCameraInfosByName.entrySet()) {
|
||||
var camPath = Paths.get(CamConfigPath.toString(), String.format("%s.json", entry.getKey()));
|
||||
if (Files.exists(camPath)) {
|
||||
try {
|
||||
// TODO: Check if deserializing correctly, if not, add CameraDeserializer
|
||||
var camJsonFile = new FileReader(camPath.toString());
|
||||
var gsonRead = new Gson().fromJson(camJsonFile, Camera.class);
|
||||
AllCamerasByName.put(entry.getKey(), gsonRead);
|
||||
} catch (FileNotFoundException ex) {
|
||||
ex.printStackTrace();
|
||||
}
|
||||
} else {
|
||||
if (!addCamera(new Camera(entry.getKey()))) {
|
||||
System.err.println("Failed to add camera! Already exists!");
|
||||
}
|
||||
}
|
||||
// TODO: Set currentCameraName from GeneralSettings instead of this
|
||||
if (currentCameraName == null && AllCamerasByName.size() == 1) { // set current camera to first found
|
||||
currentCameraName = AllCamerasByName.keySet().stream().findFirst().get();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static boolean addCamera(Camera camera) {
|
||||
if (AllCamerasByName.containsKey(camera.name)) return false;
|
||||
camera.addPipeline();
|
||||
AllCamerasByName.put(camera.name, camera);
|
||||
return true;
|
||||
}
|
||||
|
||||
public static Camera getCamera(String cameraName) {
|
||||
return AllCamerasByName.get(cameraName);
|
||||
}
|
||||
|
||||
public static void setCurrentCamera(String cameraName) throws CameraException {
|
||||
if (!AllCamerasByName.containsKey(cameraName)) throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA);
|
||||
currentCameraName = cameraName;
|
||||
SettingsManager.getInstance().updateCameraSetting(cameraName, getCurrentCamera().getCurrentPipelineIndex());
|
||||
}
|
||||
|
||||
public static Camera getCurrentCamera() throws CameraException {
|
||||
if (AllCamerasByName.size() == 0) throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA);
|
||||
var curCam = AllCamerasByName.get(currentCameraName);
|
||||
if (curCam == null) throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA);
|
||||
return curCam;
|
||||
}
|
||||
|
||||
public static void setCurrentPipeline(int pipelineNumber) throws CameraException {
|
||||
if (!getCurrentCamera().getPipelines().containsKey(pipelineNumber)) throw new CameraException(CameraException.CameraExceptionType.BAD_PIPELINE);
|
||||
getCurrentCamera().setCurrentPipelineIndex(pipelineNumber);
|
||||
SettingsManager.getInstance().updatePipelineSetting(pipelineNumber);
|
||||
}
|
||||
|
||||
public static Pipeline getCurrentPipeline() throws CameraException {
|
||||
return getCurrentCamera().getCurrentPipeline();
|
||||
}
|
||||
|
||||
public static List<String> getResolutionList() throws CameraException {
|
||||
if (!currentCameraName.equals("")) {
|
||||
List<String> list = new ArrayList<>();
|
||||
var cam = CameraManager.getCamera(currentCameraName).UsbCam;
|
||||
for (var res : cam.enumerateVideoModes()) {
|
||||
list.add(String.format("%s X %s at %s fps", res.width, res.height, res.fps));
|
||||
}
|
||||
return list;
|
||||
}
|
||||
throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA);
|
||||
}
|
||||
|
||||
public static void saveCameras() {
|
||||
for (var entry : AllCamerasByName.entrySet()) {
|
||||
try {
|
||||
Gson gson = new GsonBuilder().setPrettyPrinting().registerTypeAdapter(Camera.class, new CameraSerializer()).create();
|
||||
FileWriter writer = new FileWriter(Paths.get(CamConfigPath.toString(), String.format("%s.json", entry.getKey())).toString());
|
||||
gson.toJson(entry.getValue(), writer);
|
||||
writer.flush();
|
||||
writer.close();
|
||||
} catch (IOException ex) {
|
||||
ex.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package com.chameleonvision.vision.camera;
|
||||
import com.google.gson.*;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
|
||||
public class CameraSerializer implements JsonSerializer<Camera> {
|
||||
@Override
|
||||
public JsonElement serialize(Camera camera, Type type, JsonSerializationContext context) {
|
||||
JsonObject obj = new JsonObject();
|
||||
obj.addProperty("FOV", camera.getFOV());
|
||||
obj.addProperty("path", camera.path);
|
||||
obj.addProperty("name", camera.name);
|
||||
|
||||
var pipelines = context.serialize(camera.getPipelines());
|
||||
obj.add("pipelines", pipelines);
|
||||
|
||||
obj.addProperty("resolution", camera.getVideoModeIndex());
|
||||
obj.add("camVideoMode", context.serialize(camera.getVideoMode()));
|
||||
|
||||
return obj;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
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;
|
||||
public final double DiagonalView;
|
||||
public final Fraction AspectFraction;
|
||||
public final int HorizontalRatio;
|
||||
public final int VerticalRatio;
|
||||
public final double HorizontalView;
|
||||
public final double VerticalView;
|
||||
public final double HorizontalFocalLength;
|
||||
public final double VerticalFocalLength;
|
||||
|
||||
public CameraValues(Camera camera) {
|
||||
this(camera.getVideoMode().width, camera.getVideoMode().height, camera.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;
|
||||
DiagonalView = FastMath.toRadians(FOV);
|
||||
AspectFraction = new Fraction(ImageWidth, ImageHeight);
|
||||
HorizontalRatio = AspectFraction.getNumerator();
|
||||
VerticalRatio = AspectFraction.getDenominator();
|
||||
HorizontalView = FastMath.atan(FastMath.tan(DiagonalView / 2) * (HorizontalRatio / DiagonalView)) * 2;
|
||||
VerticalView = FastMath.atan(FastMath.tan(DiagonalView/2) * (VerticalRatio / DiagonalView)) * 2;
|
||||
HorizontalFocalLength = ImageWidth / (2 * FastMath.tan(HorizontalView /2));
|
||||
VerticalFocalLength = ImageWidth / (2 * FastMath.tan(VerticalView /2));
|
||||
}
|
||||
public double CalculatePitch(double PixelY, double centerY){
|
||||
return (FastMath.toDegrees((FastMath.atan(PixelY - centerY) / VerticalFocalLength)) * -1);
|
||||
}
|
||||
public double CalculateYaw(double PixelX, double centerX){
|
||||
return FastMath.toDegrees(FastMath.atan(PixelX - centerX) / HorizontalFocalLength);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user