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 a05f61790..f547ba71c 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 @@ -22,6 +22,7 @@ import edu.wpi.first.apriltag.jni.AprilTagJNI; import edu.wpi.first.cscore.CameraServerCvJNI; import edu.wpi.first.cscore.CameraServerJNI; import edu.wpi.first.hal.JNIWrapper; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.net.WPINetJNI; import edu.wpi.first.networktables.NetworkTablesJNI; @@ -139,6 +140,34 @@ public class TestUtils { } } + public enum WPI2023Apriltags { + k162_36_Angle, + k162_36_Straight, + k383_60_Angle2; + + public static double FOV = 68.5; + + public final Translation2d approxPose; + public final Path path; + + Path getPath() { + var filename = this.toString().substring(1); + return Path.of("2023", "AprilTags", filename + ".png"); + } + + Translation2d getPose() { + var names = this.toString().substring(1).split("_"); + var x = Units.inchesToMeters(Integer.parseInt(names[0])); + var y = Units.inchesToMeters(Integer.parseInt(names[1])); + return new Translation2d(x, y); + } + + WPI2023Apriltags() { + this.approxPose = getPose(); + this.path = getPath(); + } + } + public enum WPI2022Image { kTerminal12ft6in(Units.feetToMeters(12.5)), kTerminal22ft6in(Units.feetToMeters(22.5)); @@ -215,7 +244,7 @@ public class TestUtils { } } - private static Path getResourcesFolderPath(boolean testMode) { + public static Path getResourcesFolderPath(boolean testMode) { System.out.println("CWD: " + Path.of("").toAbsolutePath().toString()); // VSCode likes to make this path relative to the wrong root directory, so a fun hack to tell @@ -306,6 +335,7 @@ public class TestUtils { private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json"; private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json"; + public static final String LIFECAM_1280P_CAL_FILE = "lifecam_1280.json"; public static final String LIMELIGHT_480P_CAL_FILE = "limelight_1280_720.json"; public static CameraCalibrationCoefficients getCoeffs(String filename, boolean testMode) { @@ -355,4 +385,14 @@ public class TestUtils { public static void showImage(Mat frame) { showImage(frame, DefaultTimeoutMillis); } + + public static Path getTestMode2023ImagePath() { + return getResourcesFolderPath(true) + .resolve("testimages") + .resolve(WPI2022Image.kTerminal22ft6in.path); + } + + public static CameraCalibrationCoefficients get2023LifeCamCoeffs(boolean testMode) { + return getCoeffs(LIFECAM_1280P_CAL_FILE, testMode); + } } diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index e41031c65..f1eed5621 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -17,6 +17,7 @@ package org.photonvision; +import java.io.File; import java.io.IOException; import java.nio.file.Files; import java.nio.file.Path; @@ -169,24 +170,6 @@ public class Main { private static void addTestModeSources() { ConfigManager.getInstance().load(); - var camConfApril = - ConfigManager.getInstance().getConfig().getCameraConfigurations().get("Apriltag"); - if (camConfApril == null) { - camConfApril = - new CameraConfiguration("Apriltag", TestUtils.getTestModeApriltagPath().toString()); - camConfApril.FOV = TestUtils.WPI2019Image.FOV; - camConfApril.calibrations.add(TestUtils.get2019LifeCamCoeffs(true)); - - var pipeline2019 = new AprilTagPipelineSettings(); - pipeline2019.pipelineNickname = "Robots"; - pipeline2019.outputShowMultipleTargets = true; - pipeline2019.inputShouldShow = true; - - var psList2019 = new ArrayList(); - psList2019.add(pipeline2019); - camConfApril.pipelineSettings = psList2019; - } - var camConf2019 = ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2019"); if (camConf2019 == null) { @@ -245,6 +228,32 @@ public class Main { camConf2022.pipelineSettings = psList2022; } + CameraConfiguration camConf2023 = + ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2023"); + if (camConf2023 == null) { + camConf2023 = + new CameraConfiguration( + "WPI2023", + TestUtils.getResourcesFolderPath(true) + .resolve("testimages") + .resolve(TestUtils.WPI2023Apriltags.k383_60_Angle2.path) + .toString()); + + camConf2023.FOV = TestUtils.WPI2023Apriltags.FOV; + camConf2023.calibrations.add(TestUtils.get2023LifeCamCoeffs(true)); + + var pipeline2023 = new AprilTagPipelineSettings(); + var path_split = camConf2023.path.split(File.separator); + pipeline2023.pipelineNickname = path_split[path_split.length - 1].replace(".png", ""); + pipeline2023.targetModel = TargetModel.k6in_16h5; + pipeline2023.inputShouldShow = true; + pipeline2023.solvePNPEnabled = true; + + var psList2023 = new ArrayList(); + psList2023.add(pipeline2023); + camConf2023.pipelineSettings = psList2023; + } + // Colored shape testing var camConfShape = ConfigManager.getInstance().getConfig().getCameraConfigurations().get("Shape"); @@ -269,13 +278,13 @@ public class Main { var collectedSources = new ArrayList(); - var fvsApril = new FileVisionSource(camConfApril); var fvsShape = new FileVisionSource(camConfShape); var fvs2019 = new FileVisionSource(camConf2019); var fvs2020 = new FileVisionSource(camConf2020); var fvs2022 = new FileVisionSource(camConf2022); + var fvs2023 = new FileVisionSource(camConf2023); - collectedSources.add(fvsApril); + collectedSources.add(fvs2023); collectedSources.add(fvs2022); collectedSources.add(fvsShape); collectedSources.add(fvs2020); @@ -287,6 +296,7 @@ public class Main { } public static void main(String[] args) { + isTestMode = true; try { TestUtils.loadLibraries(); logger.info("Native libraries loaded."); diff --git a/test-resources/calibration/lifecam_1280.json b/test-resources/calibration/lifecam_1280.json new file mode 100644 index 000000000..64d6c07c9 --- /dev/null +++ b/test-resources/calibration/lifecam_1280.json @@ -0,0 +1,31 @@ +{ + "resolution": { + "width": 1280.0, + "height": 720.0 + }, + "cameraIntrinsics": { + "rows": 3, + "cols": 3, + "type": 6, + "data": [ + 1142.3413236140077, 0.0, 621.3842013099301, 0.0, 1139.922141261303, + 349.8976310349802, 0.0, 0.0, 1.0 + ] + }, + "cameraExtrinsics": { + "rows": 1, + "cols": 5, + "type": 6, + "data": [ + 0.1621109548975541, -1.269454756369703, 0.0027858284948980416, + -2.8060142397060727e-4, 2.502076853514765 + ] + }, + "perViewErrors": [ + 0.5169799553578814, 0.8652021086925443, 0.2641133839198221, + 0.19142021439095572, 0.30802391826048936, 0.43007079636212353, + 0.2678347488666545, 1.1019555236060878, 0.4021403723943656, + 0.3109898316125448, 0.4183600115684474, 0.4975122345222207 + ], + "standardDeviation": 0.25485508324006756 +} diff --git a/test-resources/testimages/2023/AprilTags/162_36_Angle.png b/test-resources/testimages/2023/AprilTags/162_36_Angle.png new file mode 100644 index 000000000..54a472fd3 Binary files /dev/null and b/test-resources/testimages/2023/AprilTags/162_36_Angle.png differ diff --git a/test-resources/testimages/2023/AprilTags/162_36_Straight.png b/test-resources/testimages/2023/AprilTags/162_36_Straight.png new file mode 100644 index 000000000..e2572dcbf Binary files /dev/null and b/test-resources/testimages/2023/AprilTags/162_36_Straight.png differ diff --git a/test-resources/testimages/2023/AprilTags/383_60_Angle1.png b/test-resources/testimages/2023/AprilTags/383_60_Angle1.png new file mode 100644 index 000000000..05c7d1de3 Binary files /dev/null and b/test-resources/testimages/2023/AprilTags/383_60_Angle1.png differ diff --git a/test-resources/testimages/2023/AprilTags/383_60_Angle2.png b/test-resources/testimages/2023/AprilTags/383_60_Angle2.png new file mode 100644 index 000000000..b2a1d3126 Binary files /dev/null and b/test-resources/testimages/2023/AprilTags/383_60_Angle2.png differ diff --git a/test-resources/testimages/2023/AprilTags/383_60_Straight.png b/test-resources/testimages/2023/AprilTags/383_60_Straight.png new file mode 100644 index 000000000..c5a21b2ed Binary files /dev/null and b/test-resources/testimages/2023/AprilTags/383_60_Straight.png differ diff --git a/test-resources/testimages/2023/AprilTags/ReadMe.txt b/test-resources/testimages/2023/AprilTags/ReadMe.txt new file mode 100644 index 000000000..bc9673c01 --- /dev/null +++ b/test-resources/testimages/2023/AprilTags/ReadMe.txt @@ -0,0 +1,6 @@ +All images are taken using a Microsoft Lifecam HD-3000 + +Measurements in the filename are approximate coordinates in inches, X followed by Y. The coordinate system is +the same as the Apriltag layout coordinates with the origin at the corner of the Blue Alliance wall, + Y moving across the +length of the field towards the Red Alliance Wall, +X moving across the with of the field towads the Substations (see the +Official Drawing package for a visual)