Release processed Focus mat to not leak, cache, and fix cvmat refcounting (#2356)

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/GenericUSBCameraSettables.java

@@ -169,10 +169,11 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
 
     @Override
     public void setAutoExposure(boolean cameraAutoExposure) {
-        if (configuration.cameraQuirks.hasQuirk(CameraQuirk.ArduOV9281Controls)
+        if ((configuration.cameraQuirks.hasQuirk(CameraQuirk.ArduOV9281Controls)
+                        || configuration.cameraQuirks.hasQuirk(CameraQuirk.ArduOV9782Controls))
                 && !cameraAutoExposure) {
-            // OV9281 on Linux seems to sometimes ignore our exposure requests on first boot if we're in
-            // manual mode. Poking the camera into and out of auto exposure seems to fix it.
+            // OV9281 and OV9782 on Linux seems to sometimes ignore our exposure requests on first boot if
+            // we're in manual mode. Poking the camera into and out of auto exposure seems to fix it.
             try {
                 setAutoExposureImpl(false);
                 Thread.sleep(2000);
@@ -180,7 +181,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
                 Thread.sleep(2000);
                 setAutoExposureImpl(false);
             } catch (InterruptedException e) {
-                logger.error("Thread interrupted while setting OV9281 exposure!", e);
+                logger.error("Thread interrupted while setting OV9281 or OV9782 exposure!", e);
             }
         } else {
             setAutoExposureImpl(cameraAutoExposure);

photon-core/src/main/java/org/photonvision/vision/frame/Frame.java

@@ -17,11 +17,15 @@
 
 package org.photonvision.vision.frame;
 
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.Releasable;
 
 public class Frame implements Releasable {
+    private static final Logger logger = new Logger(Frame.class, LogGroup.General);
+
     public final long sequenceID;
     public final long timestampNanos;
 
@@ -45,6 +49,15 @@ public class Frame implements Releasable {
         this.type = type;
         this.timestampNanos = timestampNanos;
         this.frameStaticProperties = frameStaticProperties;
+
+        logger.trace(
+                () ->
+                        "Allocated Frame "
+                                + sequenceID
+                                + "; color image "
+                                + colorImage.matId
+                                + "; processed "
+                                + processedImage.matId);
     }
 
     public Frame(
@@ -73,6 +86,15 @@ public class Frame implements Releasable {
 
     @Override
     public void release() {
+        logger.trace(
+                () ->
+                        "Releasing Frame "
+                                + sequenceID
+                                + "; color image "
+                                + colorImage.matId
+                                + "; processed "
+                                + processedImage.matId);
+
         colorImage.release();
         processedImage.release();
     }

photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java

@@ -24,7 +24,6 @@ import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.ImageRotationMode;
-import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
 import org.photonvision.vision.pipe.impl.GrayscalePipe;
 import org.photonvision.vision.pipe.impl.HSVPipe;
 import org.photonvision.vision.pipe.impl.RotateImagePipe;
@@ -64,26 +63,18 @@ public abstract class CpuImageProcessor extends FrameProvider {
 
     @Override
     public final Frame get() {
-        // TODO Auto-generated method stub
         var input = getInputMat();
 
+        m_rImagePipe.run(input.colorImage.getMat());
+
         CVMat outputMat = null;
-        long sumNanos = 0;
-
-        {
-            CVPipeResult<Void> out = m_rImagePipe.run(input.colorImage.getMat());
-            sumNanos += out.nanosElapsed;
-        }
-
         if (!input.colorImage.getMat().empty()) {
             if (m_processType == FrameThresholdType.HSV) {
                 var hsvResult = m_hsvPipe.run(input.colorImage.getMat());
                 outputMat = new CVMat(hsvResult.output);
-                sumNanos += hsvResult.nanosElapsed;
             } else if (m_processType == FrameThresholdType.GREYSCALE) {
                 var result = m_grayPipe.run(input.colorImage.getMat());
                 outputMat = new CVMat(result.output);
-                sumNanos += result.nanosElapsed;
             } else {
                 outputMat = new CVMat();
             }

photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java

@@ -18,21 +18,57 @@
 package org.photonvision.vision.opencv;
 
 import edu.wpi.first.util.RawFrame;
-import java.util.HashMap;
+import java.lang.ref.PhantomReference;
+import java.lang.ref.ReferenceQueue;
+import java.util.Collections;
+import java.util.Set;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.atomic.AtomicInteger;
 import org.opencv.core.Mat;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 
 public class CVMat implements Releasable {
     private static final Logger logger = new Logger(CVMat.class, LogGroup.General);
+    private static final AtomicInteger matIdCounter = new AtomicInteger(0);
 
-    private static int allMatCounter = 0;
-    private static final HashMap<Mat, Integer> allMats = new HashMap<>();
+    // All mats that have not yet been released(). these may still need to be GCed
+    private static final Set<MatTracker> allMats =
+            Collections.newSetFromMap(new ConcurrentHashMap<>());
+    private static final ReferenceQueue<CVMat> refQueue = new ReferenceQueue<>();
 
     private static boolean shouldPrint;
 
-    private final Mat mat;
-    private final RawFrame backingFrame;
+    private Mat mat;
+    private RawFrame backingFrame;
+    public final int matId;
+    private final MatTracker tracker;
+    private volatile boolean released = false;
+
+    /** Track a single CVMat instance using a PhantomReference */
+    private static class MatTracker extends PhantomReference<CVMat> {
+        final int id;
+        final long nativePtr;
+        final String allocTrace;
+        volatile boolean explicitlyReleased = false;
+
+        MatTracker(CVMat cvmat, int id, ReferenceQueue<CVMat> queue) {
+            super(cvmat, queue);
+            this.id = id;
+            this.nativePtr = cvmat.mat.nativeObj;
+            this.allocTrace = shouldPrint ? getStackTrace() : "";
+        }
+
+        private static String getStackTrace() {
+            var trace = Thread.currentThread().getStackTrace();
+            final int SKIP = 4; // Skip getStackTrace, <init>, CVMat.<init>, caller
+            var sb = new StringBuilder();
+            for (int i = SKIP; i < Math.min(trace.length, SKIP + 10); i++) {
+                sb.append("\n\t").append(trace[i]);
+            }
+            return sb.toString();
+        }
+    }
 
     public CVMat() {
         this(new Mat());
@@ -42,6 +78,19 @@ public class CVMat implements Releasable {
         this(mat, null);
     }
 
+    public CVMat(Mat mat, RawFrame frame) {
+        this.mat = mat;
+        this.backingFrame = frame;
+        this.matId = matIdCounter.incrementAndGet();
+        this.tracker = new MatTracker(this, matId, refQueue);
+
+        allMats.add(tracker);
+
+        if (shouldPrint) {
+            logger.trace("CVMat" + matId + " allocated - count: " + allMats.size() + tracker.allocTrace);
+        }
+    }
+
     public void copyFrom(CVMat srcMat) {
         copyFrom(srcMat.getMat());
     }
@@ -50,56 +99,73 @@ public class CVMat implements Releasable {
         srcMat.copyTo(mat);
     }
 
-    private StringBuilder getStackTraceBuilder() {
-        var trace = Thread.currentThread().getStackTrace();
-
-        final int STACK_FRAMES_TO_SKIP = 3;
-        final var traceStr = new StringBuilder();
-        for (int idx = STACK_FRAMES_TO_SKIP; idx < trace.length; idx++) {
-            traceStr.append("\t\n").append(trace[idx]);
-        }
-        traceStr.append("\n");
-        return traceStr;
-    }
-
-    public CVMat(Mat mat, RawFrame frame) {
-        this.mat = mat;
-        this.backingFrame = frame;
-
-        allMatCounter++;
-        allMats.put(mat, allMatCounter);
-
-        if (shouldPrint) {
-            logger.trace(() -> "CVMat" + allMatCounter + " alloc - new count: " + allMats.size());
-            logger.trace(getStackTraceBuilder()::toString);
-        }
-    }
-
     @Override
     public void release() {
-        if (this.backingFrame != null) this.backingFrame.close();
+        synchronized (this) {
+            if (released) {
+                if (shouldPrint) {
+                    logger.error("CVMat" + matId + " already released (ignored)");
+                }
+                return;
+            }
+            released = true;
+        }
 
-        // If this mat is empty, all we can do is return
-        if (mat.empty()) return;
+        tracker.explicitlyReleased = true;
 
-        // If the mat isn't in the hashmap, we can't remove it
-        Integer matNo = allMats.get(mat);
-        if (matNo != null) allMats.remove(mat);
-        mat.release();
+        // Free RawFrames exactly ONCE
+        if (backingFrame != null) {
+            try {
+                backingFrame.close();
+                backingFrame = null;
+            } catch (Exception e) {
+                logger.error("Error closing RawFrame for CVMat" + matId, e);
+            }
+        }
+
+        try {
+            if (mat != null) {
+                mat.release();
+                mat = null;
+            } else {
+                logger.error("Mat was already null, this is a no-op");
+            }
+        } catch (Exception e) {
+            logger.error("Error releasing Mat for CVMat" + matId, e);
+        }
+
+        // write down it's freed
+        allMats.remove(tracker);
 
         if (shouldPrint) {
-            logger.trace(() -> "CVMat" + matNo + " de-alloc - new count: " + allMats.size());
-            logger.trace(getStackTraceBuilder()::toString);
+            logger.trace("CVMat" + matId + " released - count: " + allMats.size());
         }
     }
 
     public Mat getMat() {
+        if (released) {
+            throw new IllegalStateException("CVMat" + matId + " has been released!");
+        }
         return mat;
     }
 
+    public boolean isReleased() {
+        return released;
+    }
+
     @Override
     public String toString() {
-        return "CVMat{" + mat.toString() + '}';
+        return "CVMat [mat="
+                + mat
+                + ", backingFrame="
+                + backingFrame
+                + ", matId="
+                + matId
+                + ", tracker="
+                + tracker
+                + ", released="
+                + released
+                + "]";
     }
 
     public static int getMatCount() {
@@ -109,4 +175,61 @@ public class CVMat implements Releasable {
     public static void enablePrint(boolean enabled) {
         shouldPrint = enabled;
     }
+
+    // todo move to somewhere else
+    static {
+        Thread cleanupThread =
+                new Thread(
+                        () -> {
+                            while (true) {
+                                try {
+                                    MatTracker ref = (MatTracker) refQueue.remove();
+
+                                    // Check if it was released before GC
+                                    if (!ref.explicitlyReleased) {
+                                        // This is a leak - remove from tracking and warn
+                                        allMats.remove(ref);
+
+                                        logger.warn(
+                                                "CVMat"
+                                                        + ref.id
+                                                        + " was GC'd without release()! "
+                                                        + "Native memory may have leaked."
+                                                        + "\nAllocated by "
+                                                        + ref.allocTrace);
+                                        if (ref.allocTrace != null) {
+                                            logger.warn("Allocated at:" + ref.allocTrace);
+                                        }
+                                    }
+
+                                    // Because we use PhantomReferences, we can't try to be nice
+                                } catch (InterruptedException e) {
+                                    Thread.currentThread().interrupt();
+                                    break;
+                                }
+                            }
+                        },
+                        "CVMat-Cleanup");
+        cleanupThread.setDaemon(true);
+        cleanupThread.start();
+    }
+
+    // Paranoia
+    @Override
+    @SuppressWarnings("deprecation")
+    protected void finalize() throws Throwable {
+        try {
+            if (!released) {
+                logger.error(
+                        "CVMat"
+                                + matId
+                                + " finalized without release()! Leaking native memory. Allocated by "
+                                + tracker.allocTrace);
+                // Don't call release() here - finalization order is unpredictable
+                // and backingFrame might already be finalized
+            }
+        } finally {
+            super.finalize();
+        }
+    }
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/FocusPipe.java

@@ -25,7 +25,9 @@ import org.opencv.imgproc.Imgproc;
 import org.photonvision.vision.pipe.CVPipe;
 
 public class FocusPipe extends CVPipe<Mat, FocusPipe.FocusResult, FocusPipe.FocusParams> {
-    private double maxVariance = 0.0;
+    // cache these
+    MatOfDouble mean = new MatOfDouble();
+    MatOfDouble stddev = new MatOfDouble();
 
     @Override
     protected FocusResult process(Mat in) {
@@ -33,8 +35,6 @@ public class FocusPipe extends CVPipe<Mat, FocusPipe.FocusResult, FocusPipe.Focu
 
         Imgproc.Laplacian(in, outputMat, CvType.CV_64F, 3);
 
-        var mean = new MatOfDouble();
-        var stddev = new MatOfDouble();
         Core.meanStdDev(outputMat, mean, stddev);
         var sd = stddev.get(0, 0)[0];
         var variance = sd * sd;

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

@@ -73,6 +73,10 @@ public class FocusPipeline extends CVPipeline<FocusPipelineResult, FocusPipeline
 
         var processedCVMat = new CVMat(displayMat);
 
+        // we no longer need the input frame's processed image, and nobody else will release it if we
+        // don't
+        frame.processedImage.release();
+
         return new FocusPipelineResult(
                 frame.sequenceID,
                 MathUtils.nanosToMillis(totalNanos),

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

@@ -191,8 +191,8 @@ public class VisionRunner {
                 // give up without increasing loop count
                 // Still feed with blank frames just dont run any pipelines
 
+                frame.release();
                 pipelineResultConsumer.accept(new CVPipelineResult(0l, 0, 0, null, new Frame()));
-
             } else if (pipeline == pipelineSupplier.get()) {
                 // If the pipeline has changed while we are getting our frame we should scrap
                 // that frame it may result in incorrect frame settings like hsv values