Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-28 02:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix calibration array length (#1879)

Browse Source 
## Description

Fixed a bug where rotated camera distortion coefficient arrays and
rotated camera intrensic arrays were 8x bigger than they were supposed
to be. The root cause was because of how the data from JsonMatOfDouble
was being handled. A Mat was pulled out, then modified, then a new
JsonMatOfDouble was made from the Mat. This required a conversion from a
Mat to a Java array. The array was incorrectly sized as `elements *
bytes`, when it should've just been `elements`. Due to some uncertainty
as to the types of Mats passed into JsonMatOfDouble.fromMat, the
expected Mat type has been explicitly documented. The added regression
test rotates the calibration data 4 times, equivalent to not rotating at
all. This should mean the arrays are equal, and that is what we check.

Additionally fixes a bug when rotating 180 degrees that flipped width
and height

## Meta

Merge checklist:
- [x] Pull Request title is [short, imperative
summary](https://cbea.ms/git-commit/) of proposed changes
- [x] The description documents the _what_ and _why_
- [ ] If this PR changes behavior or adds a feature, user documentation
is updated
- [ ] If this PR touches photon-serde, all messages have been
regenerated and hashes have not changed unexpectedly
- [ ] If this PR touches configuration, this is backwards compatible
with settings back to v2024.3.1
- [x] If this PR addresses a bug, a regression test for it is added

---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Gold856
2025-04-13 17:57:05 +00:00
committed by GitHub
parent 925defc868
commit 9a88e565fb
7 changed files with 116 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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

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

@@ -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();
}

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

@@ -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<BoardObservation> observations =
createObservations(
in,
cameraMatrixMat.getAsMat(),
cameraMatrixMat.getAsMatOfDouble(),
distortionCoefficientsMat.getAsMatOfDouble(),
rvecs,
tvecs,

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

@@ -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();

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

@@ -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

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

@@ -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());

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

@@ -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());