From b0216e17d93e23c58c49d4572ca71fa31fd39fa5 Mon Sep 17 00:00:00 2001 From: BytingBulldogs3539 Date: Sat, 4 May 2024 21:04:46 -0400 Subject: [PATCH] Update Calibrate3dPipe.java --- .../vision/pipe/impl/Calibrate3dPipe.java | 123 +++++++++--------- 1 file changed, 58 insertions(+), 65 deletions(-) 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 b9c51a9f77..8a84b78e60 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 @@ -30,6 +30,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.math.MathUtils; +import org.photonvision.mrcal.MrCalJNI; import org.photonvision.mrcal.MrCalJNI.MrCalResult; import org.photonvision.mrcal.MrCalJNILoader; import org.photonvision.vision.calibration.BoardObservation; @@ -42,10 +43,8 @@ import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult; public class Calibrate3dPipe - extends CVPipe< - Calibrate3dPipe.CalibrationInput, - CameraCalibrationCoefficients, - Calibrate3dPipe.CalibratePipeParams> { + extends + CVPipe { public static class CalibrationInput { final List observations; final FrameStaticProperties imageProps; @@ -72,33 +71,30 @@ public CalibrationInput( /** * Runs the process for the pipe. * - * @param in Input for pipe processing. In the format (Input image, object points, image points) + * @param in Input for pipe processing. In the format (Input image, object + * points, image points) * @return Result of processing. */ @Override protected CameraCalibrationCoefficients process(CalibrationInput in) { - var filteredIn = - in.observations.stream() - .filter( - it -> - it != null - && it.imagePoints != null - && it.objectPoints != null - && it.size != null) - .collect(Collectors.toList()); + var filteredIn = in.observations.stream() + .filter( + it -> it != null + && it.imagePoints != null + && it.objectPoints != null + && it.size != null) + .collect(Collectors.toList()); CameraCalibrationCoefficients ret; var start = System.nanoTime(); if (MrCalJNILoader.getInstance().isLoaded() && params.useMrCal) { logger.debug("Calibrating with mrcal!"); - ret = - calibrateMrcal( - filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength); + ret = calibrateMrcal( + filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength); } else { logger.debug("Calibrating with opencv!"); - ret = - calibrateOpenCV( - filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength); + ret = calibrateOpenCV( + filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength); } var dt = System.nanoTime() - start; @@ -113,7 +109,8 @@ protected CameraCalibrationCoefficients process(CalibrationInput in) { + "\ndistortionCoeffs:\n" + Arrays.toString(ret.distCoeffs.data) + "\n"); - else logger.info("Calibration failed! Review log for more details"); + else + logger.info("Calibration failed! Review log for more details"); return ret; } @@ -135,7 +132,7 @@ protected CameraCalibrationCoefficients calibrateOpenCV( // 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}); + 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 @@ -163,8 +160,7 @@ protected CameraCalibrationCoefficients calibrateOpenCV( JsonMatOfDouble cameraMatrixMat = JsonMatOfDouble.fromMat(cameraMatrix); JsonMatOfDouble distortionCoefficientsMat = JsonMatOfDouble.fromMat(distortionCoefficients); - var observations = - createObservations(in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null); + var observations = createObservations(in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null); cameraMatrix.release(); distortionCoefficients.release(); @@ -184,44 +180,42 @@ protected CameraCalibrationCoefficients calibrateOpenCV( protected CameraCalibrationCoefficients calibrateMrcal( List in, double fxGuess, double fyGuess) { - List corner_locations = - in.stream().map(it -> it.imagePoints).map(MatOfPoint2f::new).collect(Collectors.toList()); + List 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; - MrCalResult result = - MrCal.calibrateCamera( - corner_locations, - params.boardWidth, - params.boardHeight, - params.squareSize, - imageWidth, - imageHeight, - (fxGuess + fyGuess) / 2.0); + MrCalResult result = MrCalJNI.calibrateCamera( + corner_locations, + params.boardWidth, + params.boardHeight, + params.squareSize, + imageWidth, + imageHeight, + (fxGuess + fyGuess) / 2.0); // intrinsics are fx fy cx cy from mrcal - JsonMatOfDouble cameraMatrixMat = - new JsonMatOfDouble( - 3, - 3, - CvType.CV_64FC1, - new double[] { - // fx 0 cx - result.intrinsics[0], - 0, - result.intrinsics[2], - // 0 fy cy - 0, - result.intrinsics[1], - result.intrinsics[3], - // 0 0 1 - 0, - 0, - 1 - }); - JsonMatOfDouble distortionCoefficientsMat = - new JsonMatOfDouble(1, 8, CvType.CV_64FC1, Arrays.copyOfRange(result.intrinsics, 4, 12)); + JsonMatOfDouble cameraMatrixMat = new JsonMatOfDouble( + 3, + 3, + CvType.CV_64FC1, + new double[] { + // fx 0 cx + result.intrinsics[0], + 0, + result.intrinsics[2], + // 0 fy cy + 0, + result.intrinsics[1], + result.intrinsics[3], + // 0 0 1 + 0, + 0, + 1 + }); + 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 @@ -264,14 +258,13 @@ protected CameraCalibrationCoefficients calibrateMrcal( tvecs.add(tvec); } - List observations = - createObservations( - in, - cameraMatrixMat.getAsMat(), - distortionCoefficientsMat.getAsMatOfDouble(), - rvecs, - tvecs, - new double[] {result.warp_x, result.warp_y}); + List observations = createObservations( + in, + cameraMatrixMat.getAsMat(), + distortionCoefficientsMat.getAsMatOfDouble(), + rvecs, + tvecs, + new double[] { result.warp_x, result.warp_y }); rvecs.forEach(Mat::release); tvecs.forEach(Mat::release); @@ -280,7 +273,7 @@ protected CameraCalibrationCoefficients calibrateMrcal( in.get(0).size, cameraMatrixMat, distortionCoefficientsMat, - new double[] {result.warp_x, result.warp_y}, + new double[] { result.warp_x, result.warp_y }, observations, new Size(params.boardWidth, params.boardHeight), params.squareSize,