diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java index 2bba3c2bc..4bb7dbf9f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java @@ -116,6 +116,8 @@ public class CameraCalibrationCoefficients implements Releasable { 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 class CameraCalibrationCoefficients implements Releasable { // 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 class CameraCalibrationCoefficients implements Releasable { 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 class CameraCalibrationCoefficients implements Releasable { // 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 class CameraCalibrationCoefficients implements Releasable { rotatedIntrinsics.release(); rotatedDistCoeffs.release(); - var rotatedImageSize = new Size(unrotatedImageSize.height, unrotatedImageSize.width); - return new CameraCalibrationCoefficients( rotatedImageSize, newIntrinsics, @@ -189,7 +198,7 @@ public class CameraCalibrationCoefficients implements Releasable { @JsonIgnore public Mat getCameraIntrinsicsMat() { - return cameraIntrinsics.getAsMat(); + return cameraIntrinsics.getAsMatOfDouble(); } @JsonIgnore diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java index aa80685f9..6bae04aaf 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java @@ -58,18 +58,25 @@ public class JsonMatOfDouble implements Releasable { } @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 class JsonMatOfDouble implements Releasable { @Override public void release() { - if (wrappedMat != null) { - wrappedMat.release(); - } if (wrappedMatOfDouble != null) { wrappedMatOfDouble.release(); } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java index a7a8eeec9..428a64362 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java @@ -303,7 +303,7 @@ public class Calibrate3dPipe Calib3d.solvePnP( o.objectPoints, o.imagePoints, - cameraMatrixMat.getAsMat(), + cameraMatrixMat.getAsMatOfDouble(), distortionCoefficientsMat.getAsMatOfDouble(), rvec, tvec); @@ -314,7 +314,7 @@ public class Calibrate3dPipe List observations = createObservations( in, - cameraMatrixMat.getAsMat(), + cameraMatrixMat.getAsMatOfDouble(), distortionCoefficientsMat.getAsMatOfDouble(), rvecs, tvecs, diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java index 7c8cf2a1b..139aa9a90 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java @@ -304,7 +304,10 @@ public class Calibrate3dPipeTest { 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(); diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java index c1bb4c2d4..296e70481 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java +++ b/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 class CalibrationRotationPipeTest { } } + @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 diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java index 5a4bc4fcd..9a4d392ad 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java @@ -66,16 +66,12 @@ public class CirclePNPTest { 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()); diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java index 03bcc2535..b205a7f9d 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java @@ -70,16 +70,12 @@ public class SolvePNPTest { 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());