Free native resources in apriltag pipelines (#1272)

Addresses memory leak when switching between apriltag/aruco pipelines

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

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

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<CVMat, List<AprilTagDetection>, AprilTagDetectionPipeParams> {
-    private final AprilTagDetector m_detector = new AprilTagDetector();
+        extends CVPipe<CVMat, List<AprilTagDetection>, 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;
+    }
 }

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;

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<CVMat, List<ArucoDetectionResult>, ArucoDetectionPipeParams> {
+        extends CVPipe<CVMat, List<ArucoDetectionResult>, 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();
+    }
 }

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;

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

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java

@@ -221,4 +221,11 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
 
         return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
     }
+
+    @Override
+    public void release() {
+        aprilTagDetectionPipe.release();
+        singleTagPoseEstimatorPipe.release();
+        super.release();
+    }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java

@@ -60,8 +60,8 @@ import org.photonvision.vision.target.TrackedTarget;
 import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
 
 public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSettings> {
-    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<CVPipelineResult, ArucoPipelineSet
                 windowSize,
                 constant);
     }
+
+    @Override
+    public void release() {
+        arucoDetectionPipe.release();
+        singleTagPoseEstimatorPipe.release();
+        arucoDetectionPipe = null;
+        singleTagPoseEstimatorPipe = null;
+        super.release();
+    }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java

@@ -34,6 +34,9 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
 
     private final FrameThresholdType thresholdType;
 
+    // So releaseable doesn't keep track of if we double-free something. so (ew) remember that here
+    protected volatile boolean released = false;
+
     public CVPipeline(FrameThresholdType thresholdType) {
         this.thresholdType = thresholdType;
     }
@@ -64,6 +67,9 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
     }
 
     public R run(Frame frame, QuirkyCamera cameraQuirks) {
+        if (released) {
+            throw new RuntimeException("Pipeline use-after-free!");
+        }
         if (settings == null) {
             throw new RuntimeException("No settings provided for pipeline!");
         }
@@ -85,5 +91,7 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
      * switch. Stubbed out, but override if needed.
      */
     @Override
-    public void release() {}
+    public void release() {
+        released = true;
+    }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java

@@ -88,4 +88,9 @@ public class DriverModePipeline
                 fps,
                 new Frame(frame.processedImage, frame.colorImage, frame.type, frame.frameStaticProperties));
     }
+
+    @Override
+    public void release() {
+        // we never actually need to give resources up since pipelinemanager only makes one of us
+    }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java

@@ -131,5 +131,6 @@ public class ObjectDetectionPipeline
     @Override
     public void release() {
         rknnPipe.release();
+        super.release();
     }
 }

photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java

@@ -148,6 +148,7 @@ public class PipelineManager {
      * @return The currently active pipeline.
      */
     public CVPipeline getCurrentPipeline() {
+        updatePipelineFromRequested();
         if (currentPipelineIndex < 0) {
             switch (currentPipelineIndex) {
                 case CAL_3D_INDEX:
@@ -170,6 +171,8 @@ public class PipelineManager {
         return getPipelineSettings(currentPipelineIndex);
     }
 
+    private volatile int requestedIndex = 0;
+
     /**
      * Internal method for setting the active pipeline. <br>
      * <br>
@@ -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();
         }
 

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",