diff --git a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java index 0b15c994d..a4fea0fa7 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java @@ -18,6 +18,7 @@ package org.photonvision.common.util; import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import java.awt.HeadlessException; @@ -25,6 +26,7 @@ import java.io.File; import java.io.IOException; import java.nio.file.Path; import org.opencv.core.Mat; +import org.opencv.core.Size; import org.opencv.highgui.HighGui; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.pipeline.result.CVPipelineResult; @@ -101,6 +103,26 @@ public class TestUtils { } } + public enum WPI2026Images { + // 4000 x 1868 px + // Galaxy S23, 6.3mm focal length + kBlueOutpostFuelSpread; + + public static final Size resolution = new Size(4000, 1868); + + public static final Rotation2d FOV = Rotation2d.fromDegrees(85.0); + public final Path path; + + Path getPath() { + var filename = this.toString().substring(1); + return Path.of("2026", filename + ".jpg"); + } + + WPI2026Images() { + this.path = getPath(); + } + } + public enum WPI2024Images { kBackAmpZone_117in, kSpeakerCenter_143in; diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index 988c7cc94..b07538a58 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -18,11 +18,13 @@ package org.photonvision; import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.geometry.Rotation2d; import java.io.IOException; import java.nio.file.Path; import java.util.ArrayList; import java.util.List; import org.apache.commons.cli.*; +import org.opencv.core.Size; import org.photonvision.common.LoadJNI; import org.photonvision.common.LoadJNI.JNITypes; import org.photonvision.common.configuration.CameraConfiguration; @@ -43,7 +45,11 @@ import org.photonvision.common.networking.NetworkManager; import org.photonvision.common.util.TestUtils; import org.photonvision.server.Server; import org.photonvision.vision.apriltag.AprilTagFamily; +import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.calibration.CameraLensModel; +import org.photonvision.vision.calibration.JsonMatOfDouble; import org.photonvision.vision.camera.PVCameraInfo; +import org.photonvision.vision.frame.FrameDivisor; import org.photonvision.vision.opencv.CVMat; import org.photonvision.vision.pipeline.AprilTagPipelineSettings; import org.photonvision.vision.pipeline.CVPipelineSettings; @@ -139,36 +145,67 @@ public class Main { private static void addTestModeSources() { ConfigManager.getInstance().load(); - CameraConfiguration camConf2024 = - ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2024"); - if (camConf2024 == null || true) { - camConf2024 = + CameraConfiguration camConf2026 = + ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2026"); + if (camConf2026 == null) { + camConf2026 = new CameraConfiguration( PVCameraInfo.fromFileInfo( TestUtils.getResourcesFolderPath(true) .resolve("testimages") - .resolve(TestUtils.WPI2024Images.kSpeakerCenter_143in.path) + .resolve(TestUtils.WPI2026Images.kBlueOutpostFuelSpread.path) .toString(), - "WPI2024")); + "WPI2026")); - camConf2024.FOV = TestUtils.WPI2024Images.FOV; - // same camera as 2023 - camConf2024.calibrations.add(TestUtils.get2023LifeCamCoeffs(true)); + camConf2026.FOV = TestUtils.WPI2026Images.FOV.getDegrees(); - var pipeline2024 = new AprilTagPipelineSettings(); - var path_split = Path.of(camConf2024.matchedCameraInfo.path()).getFileName().toString(); - pipeline2024.pipelineNickname = path_split.replace(".jpg", ""); - pipeline2024.targetModel = TargetModel.kAprilTag6p5in_36h11; - pipeline2024.tagFamily = AprilTagFamily.kTag36h11; - pipeline2024.inputShouldShow = true; - pipeline2024.solvePNPEnabled = true; + // stolen from SimCameraProperties + int resWidth = (int) TestUtils.WPI2026Images.resolution.width; + int resHeight = (int) TestUtils.WPI2026Images.resolution.height; + double cx = resWidth / 2.0 - 0.5; + double cy = resHeight / 2.0 - 0.5; - var psList2024 = new ArrayList(); - psList2024.add(pipeline2024); - camConf2024.pipelineSettings = psList2024; + double resDiag = Math.hypot(resWidth, resHeight); + double diagRatio = Math.tan(TestUtils.WPI2026Images.FOV.getRadians() / 2); + var fovWidth = new Rotation2d(Math.atan(diagRatio * (resWidth / resDiag)) * 2); + var fovHeight = new Rotation2d(Math.atan(diagRatio * (resHeight / resDiag)) * 2); + + double fx = cx / Math.tan(fovWidth.getRadians() / 2.0); + double fy = cy / Math.tan(fovHeight.getRadians() / 2.0); + + JsonMatOfDouble testCameraMatrix = + new JsonMatOfDouble(3, 3, new double[] {fx, 0, cx, 0, fy, cy, 0, 0, 1}); + JsonMatOfDouble testDistortion = new JsonMatOfDouble(1, 5, new double[] {0, 0, 0, 0, 0}); + + camConf2026.calibrations.add( + new CameraCalibrationCoefficients( + new Size(4000, 1868), + testCameraMatrix, + testDistortion, + new double[0], + List.of(), + new Size(), + 1, + CameraLensModel.LENSMODEL_OPENCV)); + + logger.info("Added test camera calibration for WPI2026 " + camConf2026.calibrations); + + var pipeline2026 = new AprilTagPipelineSettings(); + var path_split = Path.of(camConf2026.matchedCameraInfo.path()).getFileName().toString(); + pipeline2026.pipelineNickname = path_split.replace(".jpg", ""); + pipeline2026.targetModel = TargetModel.kAprilTag6p5in_36h11; + pipeline2026.tagFamily = AprilTagFamily.kTag36h11; + pipeline2026.inputShouldShow = true; + pipeline2026.solvePNPEnabled = true; + pipeline2026.streamingFrameDivisor = FrameDivisor.QUARTER; + pipeline2026.decimate = 4; + + var psList2026 = new ArrayList(); + psList2026.add(pipeline2026); + camConf2026.pipelineSettings = psList2026; } - var cameraConfigs = List.of(camConf2024); + var cameraConfigs = List.of(camConf2026); ConfigManager.getInstance().unloadCameraConfigs(); cameraConfigs.stream().forEach(ConfigManager.getInstance()::addCameraConfiguration); @@ -220,7 +257,8 @@ public class Main { logger.error("Failed to parse command-line options!", e); } - // We don't want to trigger an exit in test mode or smoke test. This is specifically for MacOS. + // We don't want to trigger an exit in test mode or smoke test. This is + // specifically for MacOS. if (!(Platform.isSupported() || isSmoketest || isTestMode)) { logger.error("This platform is unsupported!"); System.exit(1); diff --git a/test-resources/testimages/2026/BlueOutpostFuelSpread.jpg b/test-resources/testimages/2026/BlueOutpostFuelSpread.jpg new file mode 100644 index 000000000..a4830af32 Binary files /dev/null and b/test-resources/testimages/2026/BlueOutpostFuelSpread.jpg differ