diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index 85f7aa829..b71fe8320 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -18,9 +18,11 @@ package org.photonvision; import edu.wpi.cscore.CameraServerCvJNI; +import java.util.ArrayList; import java.util.HashMap; import java.util.List; import org.apache.commons.cli.*; +import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.networktables.NetworkTablesManager; import org.photonvision.common.hardware.Platform; @@ -28,12 +30,16 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.LogLevel; import org.photonvision.common.logging.Logger; import org.photonvision.common.networking.NetworkManager; +import org.photonvision.common.util.TestUtils; import org.photonvision.server.Server; +import org.photonvision.vision.camera.FileVisionSource; import org.photonvision.vision.camera.USBCameraSource; import org.photonvision.vision.pipeline.CVPipelineSettings; +import org.photonvision.vision.pipeline.ReflectivePipelineSettings; import org.photonvision.vision.processes.VisionModuleManager; import org.photonvision.vision.processes.VisionSource; import org.photonvision.vision.processes.VisionSourceManager; +import org.photonvision.vision.target.TargetModel; public class Main { public static final int DEFAULT_WEBPORT = 5800; @@ -79,12 +85,12 @@ public class Main { } private static HashMap> gatherSources() { + var collectedSources = new HashMap>(); if (!isTestMode) { var camConfigs = ConfigManager.getInstance().getConfig().getCameraConfigurations(); logger.info("Loaded " + camConfigs.size() + " configs from disk."); var sources = VisionSourceManager.loadAllSources(camConfigs.values()); - var collectedSources = new HashMap>(); for (var src : sources) { var usbSrc = (USBCameraSource) src; collectedSources.put(usbSrc, usbSrc.configuration.pipelineSettings); @@ -96,11 +102,41 @@ public class Main { + usbSrc.configuration.pipelineSettings.size() + " pipelines"); } - return collectedSources; } else { - // todo: test mode - return new HashMap<>(); + var camConf2019 = + new CameraConfiguration("WPI2019", TestUtils.getTestMode2019ImagePath().toString()); + camConf2019.FOV = TestUtils.WPI2019Image.FOV; + camConf2019.calibration = TestUtils.get2019LifeCamCoeffs(true); + + var pipeline2019 = new ReflectivePipelineSettings(); + pipeline2019.pipelineNickname = "CargoShip"; + pipeline2019.targetModel = TargetModel.get2019Target(); + pipeline2019.cameraCalibration = camConf2019.calibration; + + var psList2019 = new ArrayList(); + psList2019.add(pipeline2019); + + var fvs2019 = new FileVisionSource(camConf2019); + + var camConf2020 = + new CameraConfiguration("WPI2020", TestUtils.getTestMode2020ImagePath().toString()); + camConf2020.FOV = TestUtils.WPI2020Image.FOV; + camConf2020.calibration = TestUtils.get2020LifeCamCoeffs(true); + + var pipeline2020 = new ReflectivePipelineSettings(); + pipeline2020.pipelineNickname = "OuterPort"; + pipeline2020.targetModel = TargetModel.get2020Target(); + pipeline2020.cameraCalibration = camConf2020.calibration; + + var psList2020 = new ArrayList(); + psList2020.add(pipeline2020); + + var fvs2020 = new FileVisionSource(camConf2020); + + collectedSources.put(fvs2019, psList2019); + collectedSources.put(fvs2020, psList2020); } + return collectedSources; } public static void main(String[] args) { diff --git a/photon-server/src/main/java/org/photonvision/common/util/TestUtils.java b/photon-server/src/main/java/org/photonvision/common/util/TestUtils.java index 5882a4eca..9ee7747dc 100644 --- a/photon-server/src/main/java/org/photonvision/common/util/TestUtils.java +++ b/photon-server/src/main/java/org/photonvision/common/util/TestUtils.java @@ -17,6 +17,7 @@ package org.photonvision.common.util; +import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.cscore.CameraServerCvJNI; import java.awt.*; import java.io.File; @@ -24,6 +25,7 @@ import java.io.IOException; import java.nio.file.Path; import org.opencv.core.Mat; import org.opencv.highgui.HighGui; +import org.photonvision.vision.calibration.CameraCalibrationCoefficients; public class TestUtils { @@ -39,7 +41,9 @@ public class TestUtils { kCargoStraightDark48in(1.2192), kCargoStraightDark72in(1.8288), kCargoStraightDark72in_HighRes(1.8288), - kCargoStraightDark90in(2.286); + kCargoStraightDark90in(2.286), + kRocketPanelAngleDark48in(1.2192), + kRocketPanelAngleDark60in(1.524); public static double FOV = 68.5; @@ -130,44 +134,82 @@ public class TestUtils { } } - private static Path getResourcesFolderPath() { - return Path.of("src", "test", "resources").toAbsolutePath(); + private static Path getResourcesFolderPath(boolean testMode) { + return Path.of("src", (testMode ? "main" : "test"), "resources").toAbsolutePath(); } - public static Path getTestImagesPath() { - return getResourcesFolderPath().resolve("testimages"); + public static Path getTestMode2019ImagePath() { + return getResourcesFolderPath(true) + .resolve("testimages") + .resolve(WPI2019Image.kRocketPanelAngleDark60in.path); } - public static Path getCalibrationPath() { - return getResourcesFolderPath().resolve("calibration"); + public static Path getTestMode2020ImagePath() { + return getResourcesFolderPath(true) + .resolve("testimages") + .resolve(WPI2020Image.kBlueGoal_108in_Center.path); } - public static Path getPowercellPath() { - return getTestImagesPath().resolve("polygons").resolve("powercells"); + public static Path getTestImagesPath(boolean testMode) { + return getResourcesFolderPath(testMode).resolve("testimages"); } - public static Path getWPIImagePath(WPI2020Image image) { - return getTestImagesPath().resolve(image.path); + public static Path getCalibrationPath(boolean testMode) { + return getResourcesFolderPath(testMode).resolve("calibration"); } - public static Path getWPIImagePath(WPI2019Image image) { - return getTestImagesPath().resolve(image.path); + public static Path getPowercellPath(boolean testMode) { + return getTestImagesPath(testMode).resolve("polygons").resolve("powercells"); } - public static Path getPolygonImagePath(PolygonTestImages image) { - return getTestImagesPath().resolve(image.path); + public static Path getWPIImagePath(WPI2020Image image, boolean testMode) { + return getTestImagesPath(testMode).resolve(image.path); } - public static Path getPowercellImagePath(PowercellTestImages image) { - return getPowercellPath().resolve(image.path); + public static Path getWPIImagePath(WPI2019Image image, boolean testMode) { + return getTestImagesPath(testMode).resolve(image.path); + } + + public static Path getPolygonImagePath(PolygonTestImages image, boolean testMode) { + return getTestImagesPath(testMode).resolve(image.path); + } + + public static Path getPowercellImagePath(PowercellTestImages image, boolean testMode) { + return getPowercellPath(testMode).resolve(image.path); } public static Path getDotBoardImagesPath() { - return getResourcesFolderPath().resolve("calibrationBoardImages"); + return getResourcesFolderPath(false).resolve("calibrationBoardImages"); } public static File getHardwareConfigJson() { - return getResourcesFolderPath().resolve("hardware").resolve("HardwareConfig.json").toFile(); + return getResourcesFolderPath(false) + .resolve("hardware") + .resolve("HardwareConfig.json") + .toFile(); + } + + private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json"; + private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json"; + + public static CameraCalibrationCoefficients getCoeffs(String filename, boolean testMode) { + try { + return new ObjectMapper() + .readValue( + (Path.of(getCalibrationPath(testMode).toString(), filename).toFile()), + CameraCalibrationCoefficients.class); + } catch (IOException e) { + e.printStackTrace(); + return null; + } + } + + public static CameraCalibrationCoefficients get2019LifeCamCoeffs(boolean testMode) { + return getCoeffs(LIFECAM_240P_CAL_FILE, testMode); + } + + public static CameraCalibrationCoefficients get2020LifeCamCoeffs(boolean testMode) { + return getCoeffs(LIFECAM_480P_CAL_FILE, testMode); } public static void loadLibraries() { diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java index 6bf9fbc98..f2da07b00 100644 --- a/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java +++ b/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -33,6 +33,13 @@ public class FileVisionSource implements VisionSource { private final FileFrameProvider frameProvider; private final FileSourceSettables settables; + public FileVisionSource(CameraConfiguration cameraConfiguration) { + this.cameraConfiguration = cameraConfiguration; + frameProvider = new FileFrameProvider(cameraConfiguration.path, cameraConfiguration.FOV); + settables = + new FileSourceSettables(cameraConfiguration, frameProvider.get().frameStaticProperties); + } + public FileVisionSource(String name, String imagePath, double fov) { cameraConfiguration = new CameraConfiguration(name, imagePath); frameProvider = new FileFrameProvider(imagePath, fov); diff --git a/photon-server/src/main/resources/calibration/lifecam240p.json b/photon-server/src/main/resources/calibration/lifecam240p.json new file mode 100644 index 000000000..349c450c8 --- /dev/null +++ b/photon-server/src/main/resources/calibration/lifecam240p.json @@ -0,0 +1,34 @@ +{ + "resolution": { + "width": 320.0, + "height": 240.0 + }, + "cameraIntrinsics": { + "rows": 3, + "cols": 3, + "type": 6, + "data": [ + 353.74653217742724, + 0.0, + 163.55407989211918, + 0.0, + 340.77624878700817, + 119.8945718300403, + 0.0, + 0.0, + 1.0 + ] + }, + "cameraExtrinsics": { + "rows": 1, + "cols": 5, + "type": 6, + "data": [ + 0.10322037759535845, + -0.2890556437050186, + 0.00406400648501475, + 2.5573586808275763E-4, + -1.462385758978924 + ] + } +} \ No newline at end of file diff --git a/photon-server/src/main/resources/calibration/lifecam480p.json b/photon-server/src/main/resources/calibration/lifecam480p.json new file mode 100644 index 000000000..fff629bf6 --- /dev/null +++ b/photon-server/src/main/resources/calibration/lifecam480p.json @@ -0,0 +1,34 @@ +{ + "resolution": { + "width": 640.0, + "height": 480.0 + }, + "cameraIntrinsics": { + "rows": 3, + "cols": 3, + "type": 6, + "data": [ + 699.3778103158814, + 0.0, + 345.6059345433618, + 0.0, + 677.7161226393544, + 207.12741326228522, + 0.0, + 0.0, + 1.0 + ] + }, + "cameraExtrinsics": { + "rows": 1, + "cols": 5, + "type": 6, + "data": [ + 0.14382207979312617, + -0.9851192814987014, + -0.018168751047242335, + 0.011034504043795105, + 1.9833437176538498 + ] + } +} \ No newline at end of file diff --git a/photon-server/src/main/resources/testimages/2019/WPI/RocketPanelAngleDark60in.jpg b/photon-server/src/main/resources/testimages/2019/WPI/RocketPanelAngleDark60in.jpg new file mode 100644 index 000000000..7715e503b Binary files /dev/null and b/photon-server/src/main/resources/testimages/2019/WPI/RocketPanelAngleDark60in.jpg differ diff --git a/photon-server/src/main/resources/testimages/2020/WPI/BlueGoal-108in-Center.jpg b/photon-server/src/main/resources/testimages/2020/WPI/BlueGoal-108in-Center.jpg new file mode 100644 index 000000000..6abd61899 Binary files /dev/null and b/photon-server/src/main/resources/testimages/2020/WPI/BlueGoal-108in-Center.jpg differ diff --git a/photon-server/src/test/java/org/photonvision/common/BenchmarkTest.java b/photon-server/src/test/java/org/photonvision/common/BenchmarkTest.java index 9a756729a..5277fc7fa 100644 --- a/photon-server/src/test/java/org/photonvision/common/BenchmarkTest.java +++ b/photon-server/src/test/java/org/photonvision/common/BenchmarkTest.java @@ -56,7 +56,7 @@ public class BenchmarkTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoSideStraightDark72in), + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoSideStraightDark72in, false), TestUtils.WPI2019Image.FOV); benchmarkPipeline(frameProvider, pipeline, 5); @@ -73,7 +73,7 @@ public class BenchmarkTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center), + TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center, false), TestUtils.WPI2020Image.FOV); benchmarkPipeline(frameProvider, pipeline, 5); @@ -90,7 +90,7 @@ public class BenchmarkTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center_720p), + TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center_720p, false), TestUtils.WPI2020Image.FOV); benchmarkPipeline(frameProvider, pipeline, 5); @@ -110,7 +110,7 @@ public class BenchmarkTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), TestUtils.WPI2019Image.FOV); benchmarkPipeline(frameProvider, pipeline, 5); diff --git a/photon-server/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java b/photon-server/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java index f0c3bb304..55ef65699 100644 --- a/photon-server/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java +++ b/photon-server/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java @@ -72,7 +72,7 @@ public class ShapeBenchmarkTest { pipeline.getSettings().accuracyPercentage = 30.0; var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoSideStraightDark72in), + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoSideStraightDark72in, false), TestUtils.WPI2019Image.FOV); benchmarkPipeline(frameProvider, pipeline, 5); @@ -94,7 +94,7 @@ public class ShapeBenchmarkTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center), + TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center, false), TestUtils.WPI2020Image.FOV); benchmarkPipeline(frameProvider, pipeline, 5); @@ -116,7 +116,7 @@ public class ShapeBenchmarkTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center_720p), + TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center_720p, false), TestUtils.WPI2020Image.FOV); benchmarkPipeline(frameProvider, pipeline, 5); @@ -138,7 +138,7 @@ public class ShapeBenchmarkTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), TestUtils.WPI2019Image.FOV); benchmarkPipeline(frameProvider, pipeline, 5); diff --git a/photon-server/src/test/java/org/photonvision/vision/frame/provider/FileFrameProviderTest.java b/photon-server/src/test/java/org/photonvision/vision/frame/provider/FileFrameProviderTest.java index a57effd43..4630112f9 100644 --- a/photon-server/src/test/java/org/photonvision/vision/frame/provider/FileFrameProviderTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/frame/provider/FileFrameProviderTest.java @@ -42,12 +42,13 @@ public class FileFrameProviderTest { @Test public void TestFilesExist() { - assertTrue(Files.exists(TestUtils.getTestImagesPath())); + assertTrue(Files.exists(TestUtils.getTestImagesPath(false))); } @Test public void Load2019ImageOnceTest() { - var goodFilePath = TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in); + var goodFilePath = + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in, false); assertTrue(Files.exists(goodFilePath)); @@ -79,7 +80,8 @@ public class FileFrameProviderTest { @Test public void Load2020ImageOnceTest() { - var goodFilePath = TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center); + var goodFilePath = + TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center, false); assertTrue(Files.exists(goodFilePath)); diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java index fe12c1d63..8ae2aded9 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java @@ -17,13 +17,9 @@ package org.photonvision.vision.pipeline; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.*; -import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.wpilibj.geometry.Rotation2d; -import java.io.IOException; -import java.nio.file.Path; import java.util.stream.Collectors; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; @@ -61,23 +57,13 @@ public class CirclePNPTest { } private CameraCalibrationCoefficients getCoeffs(String filename) { - try { - var cameraCalibration = - new ObjectMapper() - .readValue( - (Path.of(TestUtils.getCalibrationPath().toString(), filename).toFile()), - CameraCalibrationCoefficients.class); - - checkCameraCoefficients(cameraCalibration); - - return cameraCalibration; - } catch (IOException e) { - e.printStackTrace(); - return null; - } + var cameraCalibration = TestUtils.getCoeffs(filename, false); + checkCameraCoefficients(cameraCalibration); + return cameraCalibration; } private void checkCameraCoefficients(CameraCalibrationCoefficients cameraCalibration) { + assertNotNull(cameraCalibration); assertEquals(3, cameraCalibration.cameraIntrinsics.rows); assertEquals(3, cameraCalibration.cameraIntrinsics.cols); assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows()); @@ -124,7 +110,7 @@ public class CirclePNPTest { var frameProvider = new FileFrameProvider( - TestUtils.getPowercellImagePath(TestUtils.PowercellTestImages.kPowercell_test_6), + TestUtils.getPowercellImagePath(TestUtils.PowercellTestImages.kPowercell_test_6, false), TestUtils.WPI2020Image.FOV); CVPipelineResult pipelineResult = pipeline.run(frameProvider.get()); @@ -153,7 +139,7 @@ public class CirclePNPTest { TestUtils.loadLibraries(); var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), TestUtils.WPI2019Image.FOV); var settings = new ReflectivePipelineSettings(); diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/ColoredShapePipelineTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/ColoredShapePipelineTest.java index 49c1fb225..f19138bc3 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/ColoredShapePipelineTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/ColoredShapePipelineTest.java @@ -93,7 +93,7 @@ public class ColoredShapePipelineTest { settings.allowableThreshold = 5; var frameProvider = new FileFrameProvider( - TestUtils.getPowercellImagePath(TestUtils.PowercellTestImages.kPowercell_test_6), + TestUtils.getPowercellImagePath(TestUtils.PowercellTestImages.kPowercell_test_6, false), TestUtils.WPI2019Image.FOV); testCircleShapeDetection( pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get()); @@ -101,10 +101,11 @@ public class ColoredShapePipelineTest { public static void main(String[] args) { TestUtils.loadLibraries(); - System.out.println(TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center)); + System.out.println( + TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center, false)); var frameProvider = new FileFrameProvider( - TestUtils.getPolygonImagePath(TestUtils.PolygonTestImages.kPolygons), + TestUtils.getPolygonImagePath(TestUtils.PolygonTestImages.kPolygons, false), TestUtils.WPI2019Image.FOV); var settings = new ColoredShapePipelineSettings(); settings.hsvHue.set(0, 100); diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/ReflectivePipelineTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/ReflectivePipelineTest.java index 62aca9d39..28c854d8e 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/ReflectivePipelineTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/ReflectivePipelineTest.java @@ -43,7 +43,7 @@ public class ReflectivePipelineTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), TestUtils.WPI2019Image.FOV); TestUtils.showImage(frameProvider.get().image.getMat(), "Pipeline input", 1); @@ -71,7 +71,7 @@ public class ReflectivePipelineTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center), + TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center, false), TestUtils.WPI2020Image.FOV); CVPipelineResult pipelineResult = pipeline.run(frameProvider.get()); @@ -99,7 +99,7 @@ public class ReflectivePipelineTest { TestUtils.loadLibraries(); var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), TestUtils.WPI2019Image.FOV); var settings = new ReflectivePipelineSettings(); diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java index 39235b6db..e06516e20 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java @@ -20,10 +20,7 @@ package org.photonvision.vision.pipeline; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotNull; -import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.wpilibj.geometry.Rotation2d; -import java.io.IOException; -import java.nio.file.Path; import java.util.stream.Collectors; import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.BeforeEach; @@ -61,23 +58,13 @@ public class SolvePNPTest { } private CameraCalibrationCoefficients getCoeffs(String filename) { - try { - var cameraCalibration = - new ObjectMapper() - .readValue( - (Path.of(TestUtils.getCalibrationPath().toString(), filename).toFile()), - CameraCalibrationCoefficients.class); - - checkCameraCoefficients(cameraCalibration); - - return cameraCalibration; - } catch (IOException e) { - e.printStackTrace(); - return null; - } + var cameraCalibration = TestUtils.getCoeffs(filename, false); + checkCameraCoefficients(cameraCalibration); + return cameraCalibration; } private void checkCameraCoefficients(CameraCalibrationCoefficients cameraCalibration) { + assertNotNull(cameraCalibration); assertEquals(3, cameraCalibration.cameraIntrinsics.rows); assertEquals(3, cameraCalibration.cameraIntrinsics.cols); assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows()); @@ -114,7 +101,7 @@ public class SolvePNPTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark48in), + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark48in, false), TestUtils.WPI2019Image.FOV); CVPipelineResult pipelineResult; @@ -148,7 +135,7 @@ public class SolvePNPTest { var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_224in_Left), + TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_224in_Left, false), TestUtils.WPI2020Image.FOV); CVPipelineResult pipelineResult = pipeline.run(frameProvider.get()); @@ -163,26 +150,6 @@ public class SolvePNPTest { TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999); } - // @Test - // public void junk() { - // var frameProvider = - // new FileFrameProvider( - // - // TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), - // TestUtils.WPI2019Image.FOV); - // - // var settings = new ReflectivePipelineSettings(); - // settings.hsvHue.set(60, 100); - // settings.hsvSaturation.set(100, 255); - // settings.hsvValue.set(190, 255); - // settings.outputShowThresholded = true; - // settings.outputShowMultipleTargets = true; - // settings.contourGroupingMode = ContourGroupingMode.Dual; - // settings.contourIntersection = ContourIntersectionDirection.Up; - // - // continuouslyRunPipeline(frameProvider.getFrame(), settings); - // } - private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) { var pipeline = new ReflectivePipeline(); pipeline.settings = settings; @@ -203,7 +170,7 @@ public class SolvePNPTest { TestUtils.loadLibraries(); var frameProvider = new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), TestUtils.WPI2019Image.FOV); var settings = new ReflectivePipelineSettings(); diff --git a/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java index b35ddd37a..e45ab1b6b 100644 --- a/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java @@ -105,7 +105,8 @@ public class VisionModuleManagerTest { sources.put( new TestSource( new FileFrameProvider( - TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.getWPIImagePath( + TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), TestUtils.WPI2019Image.FOV)), List.of());