diff --git a/build.gradle b/build.gradle index d1b9cbf706..bfa31315fb 100644 --- a/build.gradle +++ b/build.gradle @@ -32,7 +32,7 @@ ext { photonGlDriverLibVersion = "dev-v2023.1.0-9-g75fc678" rknnVersion = "dev-v2024.0.0-64-gc0836a6" frcYear = "2024" - mrcalVersion = "dev-v2024.0.0-7-gc976aaa"; + mrcalVersion = "dev-v2024.0.0-29-g1536857"; pubVersion = versionString diff --git a/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue b/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue index 52f5eb93c2..c5937ab442 100644 --- a/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue +++ b/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue @@ -228,10 +228,7 @@ const calibrationImageURL = (index: number) => Board warp, X/Y {{ - useCameraSettingsStore() - .getCalibrationCoeffs(props.videoFormat.resolution) - ?.calobjectWarp?.map((it) => (it * 1000).toFixed(2) + " mm") - .join(" / ") + currentCalibrationCoeffs?.calobjectWarp?.map((it) => (it * 1000).toFixed(2) + " mm").join(" / ") }} diff --git a/photon-client/src/types/SettingTypes.ts b/photon-client/src/types/SettingTypes.ts index 0531bb8b44..6f54718eaa 100644 --- a/photon-client/src/types/SettingTypes.ts +++ b/photon-client/src/types/SettingTypes.ts @@ -129,7 +129,7 @@ export interface BoardObservation { locationInImageSpace: CvPoint[]; reprojectionErrors: CvPoint[]; optimisedCameraToObject: Pose3d; - includeObservationInCalibration: boolean; + cornersUsed: boolean[]; snapshotName: string; snapshotData: JsonImageMat; } @@ -256,7 +256,7 @@ export const PlaceholderCameraSettings: CameraSettings = { { x: 2, y: 1 }, { x: 3, y: 1 } ], - includeObservationInCalibration: false, + cornersUsed: [true, true, true, false, false, true], snapshotName: "img0.png", snapshotData: { rows: 480, cols: 640, type: CvType.CV_8U, data: "" } } diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java b/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java index 8bf41c112e..8e2b56e695 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java @@ -18,12 +18,21 @@ package org.photonvision.vision.calibration; import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnore; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.geometry.Pose3d; +import java.awt.Color; +import java.util.Arrays; import java.util.List; +import org.opencv.core.Mat; import org.opencv.core.Point; import org.opencv.core.Point3; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.photonvision.common.util.ColorHelper; +@JsonIgnoreProperties(ignoreUnknown = true) public final class BoardObservation implements Cloneable { // Expected feature 3d location in the camera frame @JsonProperty("locationInObjectSpace") @@ -42,8 +51,8 @@ public final class BoardObservation implements Cloneable { public Pose3d optimisedCameraToObject; // If we should use this observation when re-calculating camera calibration - @JsonProperty("includeObservationInCalibration") - public boolean includeObservationInCalibration; + @JsonProperty("cornersUsed") + public boolean[] cornersUsed; @JsonProperty("snapshotName") public String snapshotName; @@ -57,16 +66,22 @@ public BoardObservation( @JsonProperty("locationInImageSpace") List locationInImageSpace, @JsonProperty("reprojectionErrors") List reprojectionErrors, @JsonProperty("optimisedCameraToObject") Pose3d optimisedCameraToObject, - @JsonProperty("includeObservationInCalibration") boolean includeObservationInCalibration, + @JsonProperty("cornersUsed") boolean[] cornersUsed, @JsonProperty("snapshotName") String snapshotName, @JsonProperty("snapshotData") JsonImageMat snapshotData) { this.locationInObjectSpace = locationInObjectSpace; this.locationInImageSpace = locationInImageSpace; this.reprojectionErrors = reprojectionErrors; this.optimisedCameraToObject = optimisedCameraToObject; - this.includeObservationInCalibration = includeObservationInCalibration; this.snapshotName = snapshotName; this.snapshotData = snapshotData; + + // legacy migration -- we assume all points are inliers + if (cornersUsed == null) { + cornersUsed = new boolean[locationInObjectSpace.size()]; + Arrays.fill(cornersUsed, true); + } + this.cornersUsed = cornersUsed; } @Override @@ -79,8 +94,8 @@ public String toString() { + reprojectionErrors + ", optimisedCameraToObject=" + optimisedCameraToObject - + ", includeObservationInCalibration=" - + includeObservationInCalibration + + ", cornersUsed=" + + cornersUsed + ", snapshotName=" + snapshotName + ", snapshotData=" @@ -97,4 +112,28 @@ public BoardObservation clone() { return null; } } + + @JsonIgnore + public Mat getAnnotatedImage() { + var image = snapshotData.getAsMat().clone(); + var diag = Math.hypot(image.width(), image.height()); + int thickness = (int) Math.max(diag * 1.0 / 600.0, 1); + int r = (int) Math.max(diag * 4.0 / 500.0, 3); + var r2 = r / Math.sqrt(2); + for (int i = 0; i < this.locationInImageSpace.size(); i++) { + Scalar color; + if (cornersUsed[i]) { + color = ColorHelper.colorToScalar(Color.green); + } else { + color = ColorHelper.colorToScalar(Color.red); + } + var c = locationInImageSpace.get(i); + Imgproc.circle(image, c, r, color, thickness); + Imgproc.line( + image, new Point(c.x - r2, c.y - r2), new Point(c.x + r2, c.y + r2), color, thickness); + Imgproc.line( + image, new Point(c.x + r2, c.y - r2), new Point(c.x - r2, c.y + r2), color, thickness); + } + return image; + } } 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 1f9b32020b..dd71a69abe 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 @@ -144,8 +144,19 @@ protected CameraCalibrationCoefficients calibrateOpenCV( JsonMatOfDouble cameraMatrixMat = JsonMatOfDouble.fromMat(cameraMatrix); JsonMatOfDouble distortionCoefficientsMat = JsonMatOfDouble.fromMat(distortionCoefficients); + // Opencv is lame, so we can only assume all points are inliers + var inliners = + objPoints.stream() + .map( + it -> { + var array = new boolean[it.rows() * it.cols()]; + Arrays.fill(array, true); + return array; + }) + .collect(Collectors.toList()); + var observations = - createObservations(in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null); + createObservations(in, cameraMatrix, distortionCoefficients, rvecs, tvecs, inliners, null); cameraMatrix.release(); distortionCoefficients.release(); @@ -205,20 +216,17 @@ protected CameraCalibrationCoefficients calibrateMrcal( JsonMatOfDouble distortionCoefficientsMat = new JsonMatOfDouble(1, 8, CvType.CV_64FC1, Arrays.copyOfRange(result.intrinsics, 4, 12)); - // Calculate optimized board poses manually. We get this for free from mrcal too, but that's not - // JNIed (yet) + // Pull optimised camera to board poses out from the JNI List rvecs = new ArrayList<>(); List tvecs = new ArrayList<>(); - for (var o : in) { - var rvec = new Mat(); - var tvec = new Mat(); - Calib3d.solvePnP( - o.objectPoints, - o.imagePoints, - cameraMatrixMat.getAsMat(), - distortionCoefficientsMat.getAsMatOfDouble(), - rvec, - tvec); + for (var o : result.optimizedPoses) { + var rvec = new MatOfDouble(); + var tvec = new MatOfDouble(); + + rvec.fromArray(o.getRotation().getAxis().times(o.getRotation().getAngle()).getData()); + tvec.fromArray( + o.getTranslation().getX(), o.getTranslation().getY(), o.getTranslation().getZ()); + rvecs.add(rvec); tvecs.add(tvec); } @@ -230,6 +238,7 @@ protected CameraCalibrationCoefficients calibrateMrcal( distortionCoefficientsMat.getAsMatOfDouble(), rvecs, tvecs, + result.cornersUsed, new double[] {result.warp_x, result.warp_y}); rvecs.forEach(Mat::release); @@ -252,6 +261,7 @@ private List createObservations( MatOfDouble distortionCoefficients_, List rvecs, List tvecs, + List cornersUsed, double[] calobject_warp) { List objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList()); List imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList()); @@ -259,11 +269,11 @@ private List createObservations( // For each observation, calc reprojection error Mat jac_temp = new Mat(); List observations = new ArrayList<>(); - for (int i = 0; i < objPoints.size(); i++) { + for (int snapshotId = 0; snapshotId < objPoints.size(); snapshotId++) { MatOfPoint3f i_objPtsNative = new MatOfPoint3f(); - objPoints.get(i).copyTo(i_objPtsNative); + objPoints.get(snapshotId).copyTo(i_objPtsNative); var i_objPts = i_objPtsNative.toList(); - var i_imgPts = ((MatOfPoint2f) imgPts.get(i)).toList(); + var i_imgPts = ((MatOfPoint2f) imgPts.get(snapshotId)).toList(); // Apply warp, if set if (calobject_warp != null && calobject_warp.length == 2) { @@ -291,8 +301,8 @@ private List createObservations( try { Calib3d.projectPoints( i_objPtsNative, - rvecs.get(i), - tvecs.get(i), + rvecs.get(snapshotId), + tvecs.get(snapshotId), cameraMatrix_, distortionCoefficients_, img_pts_reprojected, @@ -313,16 +323,22 @@ private List createObservations( reprojectionError.add(error); } - var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(i), tvecs.get(i)); + var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(snapshotId), tvecs.get(snapshotId)); JsonImageMat image = null; - var inputImage = in.get(i).inputImage; + var inputImage = in.get(snapshotId).inputImage; if (inputImage != null) { image = new JsonImageMat(inputImage); } observations.add( new BoardObservation( - i_objPts, i_imgPts, reprojectionError, camToBoard, true, "img" + i + ".png", image)); + i_objPts, + i_imgPts, + reprojectionError, + camToBoard, + cornersUsed.get(snapshotId), + "img" + snapshotId + ".png", + image)); } jac_temp.release(); diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java index 0ea9f05d79..71bb82b3c6 100644 --- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java @@ -609,16 +609,16 @@ public static void onCalibrationSnapshotRequest(Context ctx) { return; } + var mat = calList.observations.get(observationIdx).getAnnotatedImage(); + // encode as jpeg to save even more space. reduces size of a 1280p image from 300k to 25k var jpegBytes = new MatOfByte(); - Imgcodecs.imencode( - ".jpg", - calList.observations.get(observationIdx).snapshotData.getAsMat(), - jpegBytes, - new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 60)); + Imgcodecs.imencode(".jpg", mat, jpegBytes, new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 60)); ctx.result(jpegBytes.toArray()); + jpegBytes.release(); + mat.release(); ctx.status(200); }