Calibration3D Pipeline Memory Leak (#185)

* Cherry-pick the extra debug info from Bank's patches.

* Updated Calibrate3d pipeline to release all unnedded mat's prior to returning.

Update a few raw mat operations to use CVMat for better traceability.

* spotless cleanup

* Added check to shouldPrint for optimization on cvmat deallocate

* Reworked stack trace printing to use lambdas for efficiency

* Missed an unneeded logger.trace

* Formatting improvements

photon-server/src/main/java/org/photonvision/Main.java

@@ -34,8 +34,10 @@ import org.photonvision.common.util.TestUtils;
 import org.photonvision.raspi.PicamJNI;
 import org.photonvision.server.Server;
 import org.photonvision.vision.camera.FileVisionSource;
+import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.ContourGroupingMode;
 import org.photonvision.vision.pipeline.CVPipelineSettings;
+import org.photonvision.vision.pipeline.PipelineProfiler;
 import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
 import org.photonvision.vision.processes.VisionModule;
 import org.photonvision.vision.processes.VisionModuleManager;
@@ -138,7 +140,10 @@ public class Main {
             logger.error("Failed to parse command-line options!", e);
         }
 
-        var logLevel = LogLevel.DEBUG;
+        CVMat.enablePrint(true);
+        PipelineProfiler.enablePrint(false);
+
+        var logLevel = printDebugLogs ? LogLevel.TRACE : LogLevel.DEBUG;
         Logger.setLevel(LogGroup.Camera, logLevel);
         Logger.setLevel(LogGroup.WebServer, logLevel);
         Logger.setLevel(LogGroup.VisionModule, logLevel);

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

@@ -17,7 +17,7 @@
 
 package org.photonvision.vision.opencv;
 
-import java.util.HashSet;
+import java.util.HashMap;
 import org.opencv.core.Mat;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
@@ -25,7 +25,8 @@ 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 HashSet<Mat> allMats = new HashSet<>();
+    private static int allMatCounter = 0;
+    private static final HashMap<Mat, Integer> allMats = new HashMap<>();
 
     private static boolean shouldPrint;
 
@@ -43,23 +44,38 @@ public class CVMat implements Releasable {
         srcMat.copyTo(mat);
     }
 
+    private StringBuilder getStackTraceBuilder() {
+        var trace = Thread.currentThread().getStackTrace();
+
+        final int STACK_FRAMES_TO_SKIP = 4;
+        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) {
         this.mat = mat;
-        if (allMats.add(mat) && shouldPrint) {
-            var trace = Thread.currentThread().getStackTrace();
+        allMatCounter++;
+        allMats.put(mat, allMatCounter);
 
-            final var traceStr = new StringBuilder();
-            for (var elem : trace) {
-                traceStr.append("\t").append(elem);
-            }
-            logger.trace(traceStr::toString);
+        if (shouldPrint) {
+            logger.trace(() -> "CVMat" + allMatCounter + " alloc - new count: " + allMats.size());
+            logger.trace(getStackTraceBuilder()::toString);
         }
     }
 
     @Override
     public void release() {
+        int matNo = allMats.get(mat);
         allMats.remove(mat);
         mat.release();
+        if (shouldPrint) {
+            logger.trace(() -> "CVMat" + matNo + " de-alloc - new count: " + allMats.size());
+            logger.trace(getStackTraceBuilder()::toString);
+        }
     }
 
     public Mat getMat() {

photon-server/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java

@@ -122,9 +122,10 @@ public class Calibrate3dPipeline
         long sumPipeNanosElapsed = 0L;
 
         // Check if the frame has chessboard corners
-        var outputColorMat = new Mat();
-        inputColorMat.copyTo(outputColorMat);
-        var findBoardResult = findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorMat)).output;
+        var outputColorCVMat = new CVMat();
+        inputColorMat.copyTo(outputColorCVMat.getMat());
+        var findBoardResult =
+                findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorCVMat.getMat())).output;
 
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
@@ -142,6 +143,7 @@ public class Calibrate3dPipeline
                 // update the UI
                 broadcastState();
 
+                outputColorCVMat.release();
                 return new CVPipelineResult(
                         MathUtils.nanosToMillis(sumPipeNanosElapsed),
                         fps,
@@ -150,12 +152,14 @@ public class Calibrate3dPipeline
             }
         }
 
+        frame.image.release();
+
         // Return the drawn chessboard if corners are found, if not, then return the input image.
         return new CVPipelineResult(
                 MathUtils.nanosToMillis(sumPipeNanosElapsed),
                 fps, // Unused but here in case
                 null,
-                new Frame(new CVMat(outputColorMat), frame.frameStaticProperties));
+                new Frame(outputColorCVMat, frame.frameStaticProperties));
     }
 
     public void deleteSavedImages() {

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

@@ -65,7 +65,7 @@ public class DriverModePipeline
         if (inputMat.channels() == 1 && PicamJNI.isSupported()) {
             long colorMatPtr = PicamJNI.grabFrame(true);
             if (colorMatPtr == 0) throw new RuntimeException("Got null Mat from GPU Picam driver");
-            inputMat.release();
+            frame.image.release();
             inputMat = new Mat(colorMatPtr);
         }
 

photon-server/src/main/java/org/photonvision/vision/pipeline/PipelineProfiler.java

@@ -22,6 +22,9 @@ import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.math.MathUtils;
 
 public class PipelineProfiler {
+
+    private static boolean shouldLog;
+
     private static final Logger reflectiveLogger =
             new Logger(ReflectivePipeline.class, LogGroup.VisionModule);
     private static final Logger coloredShapeLogger =
@@ -85,6 +88,12 @@ public class PipelineProfiler {
     }
 
     public static void printReflectiveProfile(long[] nanos) {
-        reflectiveLogger.trace(() -> getReflectiveProfileString(nanos));
+        if (shouldLog) {
+            reflectiveLogger.trace(() -> getReflectiveProfileString(nanos));
+        }
+    }
+
+    public static void enablePrint(boolean enable) {
+        shouldLog = enable;
     }
 }