Merge branch 'PhotonVision:master' into master

.github/workflows/build.yml

@@ -367,13 +367,13 @@ jobs:
           - os: ubuntu-latest
             artifact-name: LinuxArm64
             image_suffix: orangepi5
-            image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2024.0.9/photonvision_opi5.img.xz
+            image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2024.0.10/photonvision_opi5.img.xz
             cpu: cortex-a8
             image_additional_mb: 4096
           - os: ubuntu-latest
             artifact-name: LinuxArm64
             image_suffix: orangepi5plus
-            image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2024.0.9/photonvision_opi5plus.img.xz
+            image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2024.0.10/photonvision_opi5plus.img.xz
             cpu: cortex-a8
             image_additional_mb: 4096
 

photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java

@@ -23,16 +23,37 @@
 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 ArucoDetectionResult[] detect(Mat grayscaleImg) {
 
         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 @@
  * 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.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 @@ protected List<AprilTagDetection> process(CVMat in) {
             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 void setParams(AprilTagDetectionPipeParams newParams) {
 
         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.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 void setParams(AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams
         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.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 @@ private void drawCornerRefineWindow(Mat outputMat, Point corner, int windowSize)
         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.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 void setParams(ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams newPar
         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/Calibrate3dPipe.java

@@ -38,13 +38,26 @@
 import org.photonvision.vision.calibration.CameraLensModel;
 import org.photonvision.vision.calibration.JsonImageMat;
 import org.photonvision.vision.calibration.JsonMatOfDouble;
+import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.pipe.CVPipe;
+import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
 
 public class Calibrate3dPipe
         extends CVPipe<
-                List<FindBoardCornersPipe.FindBoardCornersPipeResult>,
+                Calibrate3dPipe.CalibrationInput,
                 CameraCalibrationCoefficients,
                 Calibrate3dPipe.CalibratePipeParams> {
+    public static class CalibrationInput {
+        final List<FindBoardCornersPipe.FindBoardCornersPipeResult> observations;
+        final FrameStaticProperties imageProps;
+
+        public CalibrationInput(
+                List<FindBoardCornersPipeResult> observations, FrameStaticProperties imageProps) {
+            this.observations = observations;
+            this.imageProps = imageProps;
+        }
+    }
+
     // For logging
     private static final Logger logger = new Logger(Calibrate3dPipe.class, LogGroup.General);
 
@@ -63,10 +76,9 @@ public class Calibrate3dPipe
      * @return Result of processing.
      */
     @Override
-    protected CameraCalibrationCoefficients process(
-            List<FindBoardCornersPipe.FindBoardCornersPipeResult> in) {
-        in =
-                in.stream()
+    protected CameraCalibrationCoefficients process(CalibrationInput in) {
+        var filteredIn =
+                in.observations.stream()
                         .filter(
                                 it ->
                                         it != null
@@ -79,17 +91,21 @@ protected CameraCalibrationCoefficients process(
         var start = System.nanoTime();
         if (MrCalJNILoader.getInstance().isLoaded() && params.useMrCal) {
             logger.debug("Calibrating with mrcal!");
-            ret = calibrateMrcal(in);
+            ret =
+                    calibrateMrcal(
+                            filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength);
         } else {
             logger.debug("Calibrating with opencv!");
-            ret = calibrateOpenCV(in);
+            ret =
+                    calibrateOpenCV(
+                            filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength);
         }
         var dt = System.nanoTime() - start;
 
         if (ret != null)
             logger.info(
                     "CALIBRATION SUCCESS for res "
-                            + in.get(0).size
+                            + in.observations.get(0).size
                             + " in "
                             + dt / 1e6
                             + "ms! camMatrix: \n"
@@ -103,38 +119,40 @@ protected CameraCalibrationCoefficients process(
     }
 
     protected CameraCalibrationCoefficients calibrateOpenCV(
-            List<FindBoardCornersPipe.FindBoardCornersPipeResult> in) {
+            List<FindBoardCornersPipe.FindBoardCornersPipeResult> in, double fxGuess, double fyGuess) {
         List<Mat> objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
         List<Mat> imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
         if (objPoints.size() != imgPts.size()) {
             logger.error("objpts.size != imgpts.size");
             return null;
         }
 
-        Mat cameraMatrix = new Mat();
+        Mat cameraMatrix = new Mat(3, 3, CvType.CV_64F);
         MatOfDouble distortionCoefficients = new MatOfDouble();
         List<Mat> rvecs = new ArrayList<>();
         List<Mat> tvecs = new ArrayList<>();
 
-        // RMS of the calibration
-        double calibrationAccuracy;
+        // initial camera matrix guess
+        double cx = (in.get(0).size.width / 2.0) - 0.5;
+        double cy = (in.get(0).size.width / 2.0) - 0.5;
+        cameraMatrix.put(0, 0, new double[] {fxGuess, 0, cx, 0, fyGuess, cy, 0, 0, 1});
 
         try {
             // FindBoardCorners pipe outputs all the image points, object points, and frames to calculate
             // imageSize from, other parameters are output Mats
 
-            calibrationAccuracy =
-                    Calib3d.calibrateCameraExtended(
-                            objPoints,
-                            imgPts,
-                            new Size(in.get(0).size.width, in.get(0).size.height),
-                            cameraMatrix,
-                            distortionCoefficients,
-                            rvecs,
-                            tvecs,
-                            stdDeviationsIntrinsics,
-                            stdDeviationsExtrinsics,
-                            perViewErrors);
+            Calib3d.calibrateCameraExtended(
+                    objPoints,
+                    imgPts,
+                    new Size(in.get(0).size.width, in.get(0).size.height),
+                    cameraMatrix,
+                    distortionCoefficients,
+                    rvecs,
+                    tvecs,
+                    stdDeviationsIntrinsics,
+                    stdDeviationsExtrinsics,
+                    perViewErrors,
+                    Calib3d.CALIB_USE_LU + Calib3d.CALIB_USE_INTRINSIC_GUESS);
         } catch (Exception e) {
             logger.error("Calibration failed!", e);
             e.printStackTrace();
@@ -164,13 +182,12 @@ protected CameraCalibrationCoefficients calibrateOpenCV(
     }
 
     protected CameraCalibrationCoefficients calibrateMrcal(
-            List<FindBoardCornersPipe.FindBoardCornersPipeResult> in) {
+            List<FindBoardCornersPipe.FindBoardCornersPipeResult> in, double fxGuess, double fyGuess) {
         List<MatOfPoint2f> corner_locations =
                 in.stream().map(it -> it.imagePoints).map(MatOfPoint2f::new).collect(Collectors.toList());
 
         int imageWidth = (int) in.get(0).size.width;
         int imageHeight = (int) in.get(0).size.height;
-        final double FOCAL_LENGTH_GUESS = 1200;
 
         MrCalResult result =
                 MrCalJNI.calibrateCamera(
@@ -180,7 +197,7 @@ protected CameraCalibrationCoefficients calibrateMrcal(
                         params.squareSize,
                         imageWidth,
                         imageHeight,
-                        FOCAL_LENGTH_GUESS);
+                        (fxGuess + fyGuess) / 2.0);
 
         // intrinsics are fx fy cx cy from mrcal
         JsonMatOfDouble cameraMatrixMat =

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

@@ -36,6 +36,7 @@
 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.Calibrate3dPipe.CalibrationInput;
 import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
 import org.photonvision.vision.pipeline.CVPipeline;
 import org.photonvision.vision.pipeline.Calibration3dPipelineSettings;
@@ -176,7 +177,8 @@ public CameraCalibrationCoefficients tryCalibration() {
 
         /*Pass the board corners to the pipe, which will check again to see if all boards are valid
         and returns the corresponding image and object points*/
-        calibrationOutput = calibrate3dPipe.run(foundCornersList);
+        calibrationOutput =
+                calibrate3dPipe.run(new CalibrationInput(foundCornersList, frameStaticProperties));
 
         this.calibrating = false;
 
@@ -229,4 +231,9 @@ public boolean removeSnapshot(int index) {
     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 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
 
         return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
     }
+
+    @Override
+    public void release() {
+        aprilTagDetectionPipe.release();
+        singleTagPoseEstimatorPipe.release();
+        super.release();
+    }
 }