Fix calibration array length

photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java

@@ -116,6 +116,8 @@ public CameraCalibrationCoefficients rotateCoefficients(ImageRotationMode rotati
         double p1 = getDistCoeffsMat().get(0, 2)[0];
         double p2 = getDistCoeffsMat().get(0, 3)[0];
 
+        Size rotatedImageSize = null;
+
         // A bunch of horrifying opaque rotation black magic. See image-rotation.md for more details.
         switch (rotation) {
             case DEG_0:
@@ -136,6 +138,9 @@ public CameraCalibrationCoefficients rotateCoefficients(ImageRotationMode rotati
                 // P2
                 rotatedDistCoeffs.put(0, 3, -p1);
 
+                // The rotated image size is the same as the unrotated image size, but the width and height
+                // are flipped
+                rotatedImageSize = new Size(unrotatedImageSize.height, unrotatedImageSize.width);
                 break;
             case DEG_180_CCW:
                 // CX
@@ -147,6 +152,9 @@ public CameraCalibrationCoefficients rotateCoefficients(ImageRotationMode rotati
                 rotatedDistCoeffs.put(0, 2, -p1);
                 // P2
                 rotatedDistCoeffs.put(0, 3, -p2);
+
+                // The rotated image size is the same as the unrotated image size
+                rotatedImageSize = unrotatedImageSize;
                 break;
             case DEG_90_CCW:
                 // FX
@@ -164,6 +172,9 @@ public CameraCalibrationCoefficients rotateCoefficients(ImageRotationMode rotati
                 // P2
                 rotatedDistCoeffs.put(0, 3, p1);
 
+                // The rotated image size is the same as the unrotated image size, but the width and height
+                // are flipped
+                rotatedImageSize = new Size(unrotatedImageSize.height, unrotatedImageSize.width);
                 break;
         }
 
@@ -174,8 +185,6 @@ public CameraCalibrationCoefficients rotateCoefficients(ImageRotationMode rotati
         rotatedIntrinsics.release();
         rotatedDistCoeffs.release();
 
-        var rotatedImageSize = new Size(unrotatedImageSize.height, unrotatedImageSize.width);
-
         return new CameraCalibrationCoefficients(
                 rotatedImageSize,
                 newIntrinsics,
@@ -189,7 +198,7 @@ public CameraCalibrationCoefficients rotateCoefficients(ImageRotationMode rotati
 
     @JsonIgnore
     public Mat getCameraIntrinsicsMat() {
-        return cameraIntrinsics.getAsMat();
+        return cameraIntrinsics.getAsMatOfDouble();
     }
 
     @JsonIgnore

photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java

@@ -58,18 +58,25 @@ public JsonMatOfDouble(
     }
 
     @JsonIgnore
-    public static double[] getDataFromMat(Mat mat) {
-        double[] data = new double[(int) (mat.total() * mat.elemSize())];
+    private static double[] getDataFromMat(Mat mat) {
+        double[] data = new double[(int) mat.total()];
         mat.get(0, 0, data);
         return data;
     }
 
+    /**
+     * Returns a JsonMatOfDouble by copying the data from a Mat. The Mat type must be {@link
+     * CvType#CV_64FC1}.
+     *
+     * @param mat The Mat.
+     * @return The JsonMatOfDouble
+     */
     public static JsonMatOfDouble fromMat(Mat mat) {
         return new JsonMatOfDouble(mat.rows(), mat.cols(), getDataFromMat(mat));
     }
 
     @JsonIgnore
-    public Mat getAsMat() {
+    private Mat getAsMat() {
         if (this.type != CvType.CV_64FC1) return null;
 
         if (wrappedMat == null) {
@@ -108,9 +115,6 @@ public <R extends Num, C extends Num> Matrix<R, C> getAsWpilibMat() {
 
     @Override
     public void release() {
-        if (wrappedMat != null) {
-            wrappedMat.release();
-        }
         if (wrappedMatOfDouble != null) {
             wrappedMatOfDouble.release();
         }

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

@@ -303,7 +303,7 @@ protected CameraCalibrationCoefficients calibrateMrcal(
             Calib3d.solvePnP(
                     o.objectPoints,
                     o.imagePoints,
-                    cameraMatrixMat.getAsMat(),
+                    cameraMatrixMat.getAsMatOfDouble(),
                     distortionCoefficientsMat.getAsMatOfDouble(),
                     rvec,
                     tvec);
@@ -314,7 +314,7 @@ protected CameraCalibrationCoefficients calibrateMrcal(
         List<BoardObservation> observations =
                 createObservations(
                         in,
-                        cameraMatrixMat.getAsMat(),
+                        cameraMatrixMat.getAsMatOfDouble(),
                         distortionCoefficientsMat.getAsMatOfDouble(),
                         rvecs,
                         tvecs,

photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java

@@ -304,7 +304,10 @@ private void visuallyDebugDistortion(
                 Mat raw = Imgcodecs.imread(file.getAbsolutePath());
                 Mat undistorted = new Mat(new Size(imgRes.width * 2, imgRes.height * 2), raw.type());
                 Calib3d.undistort(
-                        raw, undistorted, cal.cameraIntrinsics.getAsMat(), cal.distCoeffs.getAsMat());
+                        raw,
+                        undistorted,
+                        cal.cameraIntrinsics.getAsMatOfDouble(),
+                        cal.distCoeffs.getAsMatOfDouble());
                 TestUtils.showImage(undistorted, "undistorted " + file.getName(), 1);
                 raw.release();
                 undistorted.release();

photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java

@@ -17,6 +17,7 @@
 
 package org.photonvision.vision.pipeline;
 
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 import java.io.IOException;
@@ -173,6 +174,93 @@ public void testUndistortImagePointsWithRotation(@Enum ImageRotationMode rot) {
         }
     }
 
+    @Test
+    public void testRotateCoefficients180multiple() {
+        ImageRotationMode rot = ImageRotationMode.DEG_180_CCW;
+
+        // GIVEN A camera calibration
+        var res = new Size(1270, 720);
+        double fx = 900;
+        double fy = 951;
+        double cx = 500;
+        double cy = 321;
+        double[] intrinsics = {fx, 0, cx, 0, fy, cy, 0, 0, 1};
+        double[] distCoeffs = {
+            0.25,
+            -1.5,
+            0.0017808248356550637,
+            .00004,
+            2.179764689221826,
+            -0.034952777924711353,
+            0.09625562194891251,
+            -0.1860797479660746
+        };
+        CameraCalibrationCoefficients coeffs =
+                new CameraCalibrationCoefficients(
+                        res,
+                        new JsonMatOfDouble(3, 3, intrinsics),
+                        new JsonMatOfDouble(1, 8, distCoeffs),
+                        new double[] {},
+                        List.of(),
+                        new Size(),
+                        1,
+                        CameraLensModel.LENSMODEL_OPENCV);
+
+        // WHEN the camera calibration is rotated 180 degrees
+        var coeffs2 = coeffs.rotateCoefficients(rot);
+
+        // THEN the optical center should be rotated 180 degrees
+        double[] rotatedCamMat = {fx, 0, res.width - cx, 0, fy, res.height - cy, 0, 0, 1};
+        assertArrayEquals(rotatedCamMat, coeffs2.cameraIntrinsics.data);
+        // AND the image size should be the same
+        assertEquals(res, coeffs2.unrotatedImageSize);
+
+        // WHEN the camera calibration is rotated 180 degrees
+        var coeffs3 = coeffs2.rotateCoefficients(rot);
+
+        // THEN the camera matrix will be the same as the original
+        assertArrayEquals(intrinsics, coeffs3.cameraIntrinsics.data);
+        // AND the image size should be the same
+        assertEquals(res, coeffs2.unrotatedImageSize);
+    }
+
+    @CartesianTest
+    public void testCalibrationDataIsValidWithRotation(@Enum ImageRotationMode rot) {
+        double[] intrinsics = {
+            900, 0, 500,
+            0, 951, 321,
+            0, 0, 1
+        };
+        double[] distCoeffs = {
+            0.25,
+            -1.5,
+            0.0017808248356550637,
+            .00004,
+            2.179764689221826,
+            -0.034952777924711353,
+            0.09625562194891251,
+            -0.1860797479660746
+        };
+        // GIVEN A camera calibration
+        CameraCalibrationCoefficients coeffs =
+                new CameraCalibrationCoefficients(
+                        new Size(1270, 720),
+                        new JsonMatOfDouble(3, 3, intrinsics),
+                        new JsonMatOfDouble(1, 8, distCoeffs),
+                        new double[] {},
+                        List.of(),
+                        new Size(),
+                        1,
+                        CameraLensModel.LENSMODEL_OPENCV);
+        // WHEN A camera calibration is rotated 4 times
+        for (int i = 0; i < 4; i++) {
+            coeffs = coeffs.rotateCoefficients(rot);
+        }
+        // THEN, it should be like it was never rotated at all
+        assertArrayEquals(intrinsics, coeffs.cameraIntrinsics.data);
+        assertArrayEquals(distCoeffs, coeffs.distCoeffs.data);
+    }
+
     @Test
     public void testApriltagRotated() {
         // matt's lifecam

photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java

@@ -66,16 +66,12 @@ private void checkCameraCoefficients(CameraCalibrationCoefficients cameraCalibra
         assertNotNull(cameraCalibration);
         assertEquals(3, cameraCalibration.cameraIntrinsics.rows);
         assertEquals(3, cameraCalibration.cameraIntrinsics.cols);
-        assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows());
-        assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().cols());
         assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows());
         assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols());
         assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().rows());
         assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().cols());
         assertEquals(1, cameraCalibration.distCoeffs.rows);
         assertEquals(5, cameraCalibration.distCoeffs.cols);
-        assertEquals(1, cameraCalibration.distCoeffs.getAsMat().rows());
-        assertEquals(5, cameraCalibration.distCoeffs.getAsMat().cols());
         assertEquals(1, cameraCalibration.distCoeffs.getAsMatOfDouble().rows());
         assertEquals(5, cameraCalibration.distCoeffs.getAsMatOfDouble().cols());
         assertEquals(1, cameraCalibration.getDistCoeffsMat().rows());

photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java

@@ -70,16 +70,12 @@ private void checkCameraCoefficients(CameraCalibrationCoefficients cameraCalibra
         assertNotNull(cameraCalibration);
         assertEquals(3, cameraCalibration.cameraIntrinsics.rows);
         assertEquals(3, cameraCalibration.cameraIntrinsics.cols);
-        assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows());
-        assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().cols());
         assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows());
         assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols());
         assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().rows());
         assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().cols());
         assertEquals(1, cameraCalibration.distCoeffs.rows);
         assertEquals(5, cameraCalibration.distCoeffs.cols);
-        assertEquals(1, cameraCalibration.distCoeffs.getAsMat().rows());
-        assertEquals(5, cameraCalibration.distCoeffs.getAsMat().cols());
         assertEquals(1, cameraCalibration.distCoeffs.getAsMatOfDouble().rows());
         assertEquals(5, cameraCalibration.distCoeffs.getAsMatOfDouble().cols());
         assertEquals(1, cameraCalibration.getDistCoeffsMat().rows());