diff --git a/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java index 0093eb0c4..bc3417a21 100644 --- a/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java +++ b/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java @@ -39,6 +39,14 @@ public class MathUtils { return FastMath.atan(FastMath.toRadians(angle.doubleValue() - 90)); } + public static int safeDivide(int quotient, int divisor) { + if (divisor == 0) { + return 0; + } else { + return quotient / divisor; + } + } + public static double roundTo(double value, int to) { double toMult = Math.pow(10, to); return (double) Math.round(value * toMult) / toMult; diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/CameraQuirk.java b/photon-server/src/main/java/org/photonvision/vision/camera/CameraQuirk.java index 541fbc448..9a558f721 100644 --- a/photon-server/src/main/java/org/photonvision/vision/camera/CameraQuirk.java +++ b/photon-server/src/main/java/org/photonvision/vision/camera/CameraQuirk.java @@ -20,8 +20,6 @@ package org.photonvision.vision.camera; public enum CameraQuirk { /** Camera settable for controllable image gain */ Gain, - /** For cameras that need a bit of encouragement for settings to stick */ - DoubleSet, - /** For cameras that are pi cams */ + /** For the Raspberry Pi Camera */ PiCam } diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java b/photon-server/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java index e35e8353a..1594702c9 100644 --- a/photon-server/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java +++ b/photon-server/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java @@ -25,22 +25,37 @@ public class QuirkyCamera { private static final List quirkyCameras = List.of( - new QuirkyCamera(0x2000, 0x1415, "PS3Eye", CameraQuirk.Gain), - new QuirkyCamera(0x72E, 0x45D, "LifeCam VX-5500", CameraQuirk.DoubleSet), - new QuirkyCamera(-1, -1, "PiCam", CameraQuirk.PiCam)); + new QuirkyCamera(0x2000, 0x1415, CameraQuirk.Gain), // PS3Eye + new QuirkyCamera(-1, -1, "mmal service 16.1", CameraQuirk.PiCam) // PiCam + ); - public static final QuirkyCamera DefaultCamera = new QuirkyCamera(0, 0, "", List.of()); + public static final QuirkyCamera DefaultCamera = new QuirkyCamera(0, 0, ""); + public final String baseName; public final int usbVid; public final int usbPid; - public final String baseName; public final HashMap quirks; - private QuirkyCamera(int usbVid, int usbPid, String baseName, CameraQuirk quirk) { - this(usbVid, usbPid, baseName, List.of(quirk)); + /** + * Creates a QuirkyCamera that matches by USB VID/PID + * + * @param usbVid USB VID of camera + * @param usbPid USB PID of camera + * @param quirks Camera quirks + */ + private QuirkyCamera(int usbVid, int usbPid, CameraQuirk... quirks) { + this(usbVid, usbPid, "", quirks); } - private QuirkyCamera(int usbVid, int usbPid, String baseName, List quirks) { + /** + * Creates a QuirkyCamera that matches by USB VID/PID and name + * + * @param usbVid USB VID of camera + * @param usbPid USB PID of camera + * @param baseName CSCore name of camera + * @param quirks Camera quirks + */ + private QuirkyCamera(int usbVid, int usbPid, String baseName, CameraQuirk... quirks) { this.usbVid = usbVid; this.usbPid = usbPid; this.baseName = baseName; @@ -58,13 +73,23 @@ public class QuirkyCamera { return quirks.get(quirk); } + public static QuirkyCamera getQuirkyCamera(int usbVid, int usbPid) { + return getQuirkyCamera(usbVid, usbPid, ""); + } + public static QuirkyCamera getQuirkyCamera(int usbVid, int usbPid, String baseName) { for (var qc : quirkyCameras) { - if (qc.usbVid == usbVid && qc.usbPid == usbPid) { + boolean hasBaseName = !qc.baseName.equals(""); + boolean matchesBaseName = qc.baseName.equals(baseName) || !hasBaseName; + if (qc.usbVid == usbVid && qc.usbPid == usbPid && matchesBaseName) { return qc; } } - return new QuirkyCamera(usbVid, usbPid, baseName, List.of()); + return new QuirkyCamera(usbVid, usbPid, baseName); + } + + public boolean hasQuirks() { + return quirks.containsValue(true); } @Override diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java index 33ece846a..5c9575976 100644 --- a/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java +++ b/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java @@ -26,6 +26,7 @@ import java.util.*; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.frame.FrameProvider; import org.photonvision.vision.frame.provider.USBFrameProvider; import org.photonvision.vision.processes.VisionSource; @@ -45,10 +46,16 @@ public class USBCameraSource implements VisionSource { logger = new Logger(USBCameraSource.class, config.nickname, LogGroup.Camera); configuration = config; camera = new UsbCamera(config.nickname, config.path); + cvSink = CameraServer.getInstance().getVideo(this.camera); + cameraQuirks = QuirkyCamera.getQuirkyCamera( camera.getInfo().productId, camera.getInfo().vendorId, config.baseName); - cvSink = CameraServer.getInstance().getVideo(this.camera); + + if (cameraQuirks.hasQuirks()) { + logger.info("Quirky camera detected: " + cameraQuirks.baseName); + } + usbCameraSettables = new USBCameraSettables(config); usbFrameProvider = new USBFrameProvider(cvSink, usbCameraSettables); } @@ -74,8 +81,15 @@ public class USBCameraSource implements VisionSource { @Override public void setExposure(int exposure) { try { - camera.setExposureManual(exposure); - camera.setExposureManual(exposure); + if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) { + camera.getProperty("auto_exposure").set(1); // auto exposure off + camera + .getProperty("exposure_time_absolute") + .set(MathUtils.safeDivide(10000, exposure)); // exposure time is a range, 0-10000 + } else { + camera.setExposureManual(exposure); + camera.setExposureManual(exposure); + } } catch (VideoException e) { logger.error("Failed to set camera exposure!", e); } diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java b/photon-server/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java index e41f8d214..ab0106281 100644 --- a/photon-server/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java +++ b/photon-server/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj.geometry.Rotation2d; import org.apache.commons.math3.fraction.Fraction; import org.apache.commons.math3.util.FastMath; import org.opencv.core.Point; +import org.photonvision.common.util.numbers.DoubleCouple; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; /** Represents the properties of a frame. */ @@ -75,8 +76,17 @@ public class FrameStaticProperties { centerPoint = new Point(centerX, centerY); // pinhole model calculations - double diagonalView = FastMath.toRadians(this.fov); - Fraction aspectFraction = new Fraction(this.imageWidth, this.imageHeight); + DoubleCouple horizVertViews = + calculateHorizontalVerticalFoV(this.fov, this.imageWidth, this.imageHeight); + + horizontalFocalLength = this.imageWidth / (2 * FastMath.tan(horizVertViews.getFirst() / 2)); + verticalFocalLength = this.imageHeight / (2 * FastMath.tan(horizVertViews.getSecond() / 2)); + } + + public static DoubleCouple calculateHorizontalVerticalFoV( + double diagonalFoV, int imageWidth, int imageHeight) { + double diagonalView = FastMath.toRadians(diagonalFoV); + Fraction aspectFraction = new Fraction(imageWidth, imageHeight); int horizontalRatio = aspectFraction.getNumerator(); int verticalRatio = aspectFraction.getDenominator(); @@ -87,7 +97,6 @@ public class FrameStaticProperties { 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)); + return new DoubleCouple(horizontalView, verticalView); } } diff --git a/photon-server/src/test/java/org/photonvision/vision/QuirkyCameraTest.java b/photon-server/src/test/java/org/photonvision/vision/QuirkyCameraTest.java index 653ca3f6f..96e53e63a 100644 --- a/photon-server/src/test/java/org/photonvision/vision/QuirkyCameraTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/QuirkyCameraTest.java @@ -32,10 +32,22 @@ public class QuirkyCameraTest { ps3EyeQuirks.putIfAbsent(q, false); } - QuirkyCamera psEye = QuirkyCamera.getQuirkyCamera(0x2000, 0x1415, "psEye"); + QuirkyCamera psEye = QuirkyCamera.getQuirkyCamera(0x2000, 0x1415); Assertions.assertEquals(psEye.quirks, ps3EyeQuirks); } + @Test + public void picamTest() { + HashMap picamQuirks = new HashMap<>(); + picamQuirks.put(CameraQuirk.PiCam, true); + for (var q : CameraQuirk.values()) { + picamQuirks.putIfAbsent(q, false); + } + + QuirkyCamera picam = QuirkyCamera.getQuirkyCamera(-1, -1, "mmal service 16.1"); + Assertions.assertEquals(picam.quirks, picamQuirks); + } + @Test public void quirklessCameraTest() { HashMap noQuirks = new HashMap<>(); @@ -43,7 +55,7 @@ public class QuirkyCameraTest { noQuirks.put(q, false); } - QuirkyCamera quirkless = QuirkyCamera.getQuirkyCamera(1234, 888, "empty"); + QuirkyCamera quirkless = QuirkyCamera.getQuirkyCamera(1234, 8888); Assertions.assertEquals(quirkless.quirks, noQuirks); } } diff --git a/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java b/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java index 5c938ad32..49c169083 100644 --- a/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java @@ -28,6 +28,7 @@ import org.opencv.core.*; import org.opencv.core.Point; import org.opencv.imgproc.Imgproc; import org.photonvision.common.util.TestUtils; +import org.photonvision.common.util.numbers.DoubleCouple; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.opencv.DualOffsetValues; @@ -176,6 +177,16 @@ public class TargetCalculationsTest { assertEquals(-70, result, 0.01); } + @Test + public void testCameraFOVCalculation() { + final DoubleCouple glowormHorizVert = + FrameStaticProperties.calculateHorizontalVerticalFoV(74.8, 640, 480); + var gwHorizDeg = FastMath.toDegrees(glowormHorizVert.getFirst()); + var gwVertDeg = FastMath.toDegrees(glowormHorizVert.getSecond()); + assertEquals(62.7, gwHorizDeg, .3); + assertEquals(49, gwVertDeg, .3); + } + @Test public void robotOffsetDualTest() { final DualOffsetValues dualOffsetValues =