Add 2023 pics to test mode (#720)

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);
+    }
 }

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<CVPipelineSettings>();
-            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<CVPipelineSettings>();
+            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<VisionSource>();
 
-        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.");

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
+}

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)