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 8a84b78e60..dc43dc14bf 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 @@ -43,8 +43,10 @@ import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult; public class Calibrate3dPipe - extends - CVPipe { + extends CVPipe< + Calibrate3dPipe.CalibrationInput, + CameraCalibrationCoefficients, + Calibrate3dPipe.CalibratePipeParams> { public static class CalibrationInput { final List observations; final FrameStaticProperties imageProps; @@ -71,30 +73,33 @@ 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; @@ -109,8 +114,7 @@ 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; } @@ -132,7 +136,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 @@ -160,7 +164,8 @@ 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(); @@ -180,42 +185,44 @@ 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 = MrCalJNI.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 @@ -258,13 +265,14 @@ 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); @@ -273,7 +281,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,