From f6736fc7300af1f9b79792c2edd2bcf5839375df Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Thu, 13 Mar 2025 19:50:41 -0400 Subject: [PATCH] Force load opencv before using OpenCV functions (#1808) Force loads OpenCV before any OpenCV functions are used. `OpenCVLoader` has all of its loading done in a static initializer field, so it's only loaded once. Also deprecates `OpenCVHelp.forceLoadOpenCV()`, since it's functionality is the exact same. Resolves #1803 --- .../java/org/photonvision/PhotonPoseEstimator.java | 11 +++++++++++ .../photonvision/simulation/PhotonCameraSim.java | 3 ++- .../org/photonvision/simulation/VideoSimUtil.java | 3 ++- .../java/org/photonvision/VisionSystemSimTest.java | 4 ++-- .../org/photonvision/estimation/OpenCVHelp.java | 14 ++++++-------- .../photonvision/estimation/VisionEstimation.java | 5 +++++ 6 files changed, 28 insertions(+), 12 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 7ef835ca5..a389d5931 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -25,6 +25,7 @@ package org.photonvision; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.cscore.OpenCvLoader; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; @@ -159,6 +160,11 @@ public class PhotonPoseEstimator { this.primaryStrategy = strategy; this.robotToCamera = robotToCamera; + if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO + || strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) { + OpenCvLoader.forceStaticLoad(); + } + HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); InstanceCount++; } @@ -231,6 +237,11 @@ public class PhotonPoseEstimator { */ public void setPrimaryStrategy(PoseStrategy strategy) { checkUpdate(this.primaryStrategy, strategy); + + if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO + || strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) { + OpenCvLoader.forceStaticLoad(); + } this.primaryStrategy = strategy; } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 2bd7cfc78..3493d3a92 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -28,6 +28,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.cscore.OpenCvLoader; import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; @@ -94,7 +95,7 @@ public class PhotonCameraSim implements AutoCloseable { private boolean videoSimProcEnabled = true; static { - OpenCVHelp.forceLoadOpenCV(); + OpenCvLoader.forceStaticLoad(); } @Override diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index da725eb8f..325a2ab2c 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -26,6 +26,7 @@ package org.photonvision.simulation; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.cscore.OpenCvLoader; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; @@ -62,7 +63,7 @@ public class VideoSimUtil { private static double fieldWidth = 8.0137; static { - OpenCVHelp.forceLoadOpenCV(); + OpenCvLoader.forceStaticLoad(); // create Mats of 10x10 apriltag images for (int i = 0; i < VideoSimUtil.kNumTags36h11; i++) { diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index fd7f79814..d4c324965 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -33,6 +33,7 @@ import static org.photonvision.UnitTestUtils.waitForSequenceNumber; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.cscore.OpenCvLoader; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -57,7 +58,6 @@ import org.junit.jupiter.params.ParameterizedTest; import org.junit.jupiter.params.provider.Arguments; import org.junit.jupiter.params.provider.MethodSource; import org.junit.jupiter.params.provider.ValueSource; -import org.photonvision.estimation.OpenCVHelp; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; import org.photonvision.jni.PhotonTargetingJniLoader; @@ -84,7 +84,7 @@ class VisionSystemSimTest { fail(e); } - OpenCVHelp.forceLoadOpenCV(); + OpenCvLoader.forceStaticLoad(); // See #1574 - test flakey, disabled until we address this assumeTrue(false); diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 1799c14fa..372a5e7cf 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -17,7 +17,7 @@ package org.photonvision.estimation; -import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.cscore.OpenCvLoader; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -54,14 +54,12 @@ public final class OpenCVHelp { private static final Rotation3d NWU_TO_EDN; private static final Rotation3d EDN_TO_NWU; - // Creating a cscore object is sufficient to load opencv, per - // https://www.chiefdelphi.com/t/unsatisfied-link-error-when-simulating-java-robot-code-using-opencv/426731/4 - private static CvSink dummySink = null; - + /** + * @deprecated Replaced by {@link OpenCvLoader#forceStaticLoad()} + */ + @Deprecated(since = "2025", forRemoval = true) public static void forceLoadOpenCV() { - if (dummySink != null) return; - dummySink = new CvSink("ignored"); - dummySink.close(); + OpenCvLoader.forceStaticLoad(); } static { diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index 49067234f..6031ec337 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -19,6 +19,7 @@ package org.photonvision.estimation; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.cscore.OpenCvLoader; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -105,6 +106,8 @@ public class VisionEstimation { if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) { return Optional.empty(); } + OpenCvLoader.forceStaticLoad(); + Point[] points = OpenCVHelp.cornersToPoints(corners); // single-tag pnp @@ -200,6 +203,8 @@ public class VisionEstimation { if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) { return Optional.empty(); } + OpenCvLoader.forceStaticLoad(); + Point[] points = OpenCVHelp.cornersToPoints(corners); // Undistort