diff --git a/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java b/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java index 3be4cf093..e467f2158 100644 --- a/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java +++ b/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java @@ -23,16 +23,37 @@ import java.util.Comparator; import org.opencv.core.Mat; import org.opencv.objdetect.ArucoDetector; import org.opencv.objdetect.DetectorParameters; +import org.opencv.objdetect.Dictionary; import org.opencv.objdetect.Objdetect; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.vision.opencv.Releasable; /** This class wraps an {@link ArucoDetector} for convenience. */ -public class PhotonArucoDetector { +public class PhotonArucoDetector implements Releasable { private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule); - private final ArucoDetector detector = - new ArucoDetector(Objdetect.getPredefinedDictionary(Objdetect.DICT_APRILTAG_16h5)); + private static class ArucoDetectorHack extends ArucoDetector { + public ArucoDetectorHack(Dictionary predefinedDictionary) { + super(predefinedDictionary); + } + + // avoid double-free by keeping track of this ourselves (ew) + private boolean freed = false; + + @Override + public void finalize() throws Throwable { + if (freed) { + return; + } + + super.finalize(); + freed = true; + } + } + + private final ArucoDetectorHack detector = + new ArucoDetectorHack(Objdetect.getPredefinedDictionary(Objdetect.DICT_APRILTAG_16h5)); private final Mat ids = new Mat(); private final ArrayList cornerMats = new ArrayList<>(); @@ -95,4 +116,16 @@ public class PhotonArucoDetector { return results; } + + @Override + public void release() { + try { + detector.finalize(); + } catch (Throwable e) { + logger.error("Exception destroying PhotonArucoDetector", e); + } + ids.release(); + for (var m : cornerMats) m.release(); + cornerMats.clear(); + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java index 315e4e3de..83a947726 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java @@ -33,7 +33,7 @@ import org.photonvision.vision.opencv.CVMat; * path}. */ public class FileFrameProvider extends CpuImageProcessor { - public static final int MAX_FPS = 5; + public static final int MAX_FPS = 10; private static int count = 0; private final int thisIndex = count++; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java index 5c1497db3..7931dbcb0 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java @@ -21,11 +21,13 @@ import edu.wpi.first.apriltag.AprilTagDetection; import edu.wpi.first.apriltag.AprilTagDetector; import java.util.List; import org.photonvision.vision.opencv.CVMat; +import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.pipe.CVPipe; public class AprilTagDetectionPipe - extends CVPipe, AprilTagDetectionPipeParams> { - private final AprilTagDetector m_detector = new AprilTagDetector(); + extends CVPipe, AprilTagDetectionPipeParams> + implements Releasable { + private AprilTagDetector m_detector = new AprilTagDetector(); public AprilTagDetectionPipe() { super(); @@ -40,6 +42,10 @@ public class AprilTagDetectionPipe return List.of(); } + if (m_detector == null) { + throw new RuntimeException("Apriltag detector was released!"); + } + var ret = m_detector.detect(in.getMat()); if (ret == null) { @@ -60,4 +66,10 @@ public class AprilTagDetectionPipe super.setParams(newParams); } + + @Override + public void release() { + m_detector.close(); + m_detector = null; + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java index 3c6563e18..3ddc2f3de 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java @@ -25,13 +25,15 @@ import org.opencv.calib3d.Calib3d; import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.pipe.CVPipe; public class AprilTagPoseEstimatorPipe extends CVPipe< AprilTagDetection, AprilTagPoseEstimate, - AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams> { + AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams> + implements Releasable { private final AprilTagPoseEstimator m_poseEstimator = new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0, 0, 0, 0, 0)); @@ -92,6 +94,11 @@ public class AprilTagPoseEstimatorPipe super.setParams(newParams); } + @Override + public void release() { + temp.release(); + } + public static class AprilTagPoseEstimatorPipeParams { final AprilTagPoseEstimator.Config config; final CameraCalibrationCoefficients calibration; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java index 4d0cdb55e..1b39fd2e4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java @@ -29,10 +29,12 @@ import org.opencv.objdetect.Objdetect; import org.photonvision.vision.aruco.ArucoDetectionResult; import org.photonvision.vision.aruco.PhotonArucoDetector; import org.photonvision.vision.opencv.CVMat; +import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.pipe.CVPipe; public class ArucoDetectionPipe - extends CVPipe, ArucoDetectionPipeParams> { + extends CVPipe, ArucoDetectionPipeParams> + implements Releasable { // ArucoDetector wrapper class private final PhotonArucoDetector photonDetector = new PhotonArucoDetector(); @@ -131,4 +133,9 @@ public class ArucoDetectionPipe var pt2 = new Point(corner.x + windowSize, corner.y + windowSize); Imgproc.rectangle(outputMat, pt1, pt2, new Scalar(0, 0, 255), thickness); } + + @Override + public void release() { + photonDetector.release(); + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java index 579996890..bc168c6a6 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java @@ -32,13 +32,15 @@ import org.opencv.core.MatOfPoint3f; import org.opencv.core.Point3; import org.photonvision.vision.aruco.ArucoDetectionResult; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.pipe.CVPipe; public class ArucoPoseEstimatorPipe extends CVPipe< ArucoDetectionResult, AprilTagPoseEstimate, - ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams> { + ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams> + implements Releasable { // image points of marker corners private final MatOfPoint2f imagePoints = new MatOfPoint2f(Mat.zeros(4, 1, CvType.CV_32FC2)); // rvec/tvec estimations from solvepnp @@ -117,6 +119,18 @@ public class ArucoPoseEstimatorPipe super.setParams(newParams); } + @Override + public void release() { + imagePoints.release(); + for (var m : rvecs) m.release(); + rvecs.clear(); + for (var m : tvecs) m.release(); + tvecs.clear(); + rvec.release(); + tvec.release(); + reprojectionErrors.release(); + } + public static class ArucoPoseEstimatorPipeParams { final CameraCalibrationCoefficients calibration; final double tagSize; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipeline.java index a5b9f17f6..bc9392e01 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipeline.java @@ -231,4 +231,9 @@ public class Calibrate3dPipeline public CameraCalibrationCoefficients cameraCalibrationCoefficients() { return calibrationOutput.output; } + + @Override + public void release() { + // we never actually need to give resources up since pipelinemanager only makes one of us + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index 5d2abe1a0..7f69af66c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -221,4 +221,11 @@ public class AprilTagPipeline extends CVPipeline { - private final ArucoDetectionPipe arucoDetectionPipe = new ArucoDetectionPipe(); - private final ArucoPoseEstimatorPipe singleTagPoseEstimatorPipe = new ArucoPoseEstimatorPipe(); + private ArucoDetectionPipe arucoDetectionPipe = new ArucoDetectionPipe(); + private ArucoPoseEstimatorPipe singleTagPoseEstimatorPipe = new ArucoPoseEstimatorPipe(); private final MultiTargetPNPPipe multiTagPNPPipe = new MultiTargetPNPPipe(); private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe(); @@ -250,4 +250,13 @@ public class ArucoPipeline extends CVPipeline *
@@ -179,6 +182,22 @@ public class PipelineManager { * @param newIndex Index of pipeline to be active */ private void setPipelineInternal(int newIndex) { + requestedIndex = newIndex; + } + + /** + * Based on a requested pipeline index, create/destroy pipelines as necessary. We do this as a + * side effect of the main thread that calls getCurrentPipeline to avoid race conditions between + * server threads and the VisionRunner TODO: this should be refactored. Shame Java doesn't have + * RAII + */ + private void updatePipelineFromRequested() { + int newIndex = requestedIndex; + if (newIndex == currentPipelineIndex) { + // nothing to do, probably no change -- give up + return; + } + if (newIndex < 0 && currentPipelineIndex >= 0) { // Transitioning to a built-in pipe, save off the current user one lastUserPipelineIdx = currentPipelineIndex; @@ -189,8 +208,8 @@ public class PipelineManager { return; } - // Cleanup potential old native resources before swapping over - if (currentUserPipeline != null) { + // Cleanup potential old native resources before swapping over for user pipelines + if (currentUserPipeline != null && !(newIndex < 0)) { currentUserPipeline.release(); } diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index 858813659..52a3225be 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -277,7 +277,7 @@ public class Main { CameraConfiguration camConf2024 = ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2024"); - if (camConf2024 == null || true) { + if (camConf2024 == null) { camConf2024 = new CameraConfiguration( "WPI2024",