Skip to content

Commit

Permalink
Update Calibrate3dPipe.java
Browse files Browse the repository at this point in the history
  • Loading branch information
@BytingBulldogs3539
BytingBulldogs3539 committed May 5, 2024
1 parent ce00190 commit b0216e1
Showing 1 changed file with 58 additions and 65 deletions.
123 changes: 58 additions & 65 deletions photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -42,10 +43,8 @@
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;

public class Calibrate3dPipe
extends CVPipe<
Calibrate3dPipe.CalibrationInput,
CameraCalibrationCoefficients,
Calibrate3dPipe.CalibratePipeParams> {
extends
CVPipe<Calibrate3dPipe.CalibrationInput, CameraCalibrationCoefficients, Calibrate3dPipe.CalibratePipeParams> {
public static class CalibrationInput {
final List<FindBoardCornersPipe.FindBoardCornersPipeResult> observations;
final FrameStaticProperties imageProps;
Expand All @@ -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;

Expand All @@ -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;
}
Expand All @@ -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
Expand Down Expand Up @@ -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();
Expand All @@ -184,44 +180,42 @@ protected CameraCalibrationCoefficients calibrateOpenCV(

protected CameraCalibrationCoefficients calibrateMrcal(
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in, double fxGuess, double fyGuess) {
List<MatOfPoint2f> corner_locations =
in.stream().map(it -> it.imagePoints).map(MatOfPoint2f::new).collect(Collectors.toList());
List<MatOfPoint2f> 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
Expand Down Expand Up @@ -264,14 +258,13 @@ protected CameraCalibrationCoefficients calibrateMrcal(
tvecs.add(tvec);
}

List<BoardObservation> observations =
createObservations(
in,
cameraMatrixMat.getAsMat(),
distortionCoefficientsMat.getAsMatOfDouble(),
rvecs,
tvecs,
new double[] {result.warp_x, result.warp_y});
List<BoardObservation> 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);
Expand All @@ -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,
Expand Down

0 comments on commit b0216e1

Please sign in to comment.