Skip to content

Commit

Permalink
Format, Add levels / mrcal support.
Browse files Browse the repository at this point in the history
  • Loading branch information
@BytingBulldogs3539
BytingBulldogs3539 committed May 4, 2024
1 parent 39b97fa commit ce00190
Show file tree
Hide file tree
Showing 8 changed files with 257 additions and 206 deletions.
5 changes: 3 additions & 2 deletions photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,9 @@ public USBFrameProvider(CvSink sink, VisionSourceSettables visionSettables) {
public CapturedFrame getInputMat() {
var mat = new CVMat(); // We do this so that we don't fill a Mat in use by another thread
// This is from wpi::Now, or WPIUtilJNI.now()
long time = cvSink.grabFrame(mat.getMat())
* 1000; // Units are microseconds, epoch is the same as the Unix epoch
long time =
cvSink.grabFrame(mat.getMat())
* 1000; // Units are microseconds, epoch is the same as the Unix epoch

if (time == 0) {
var error = cvSink.getError();
Expand Down
37 changes: 31 additions & 6 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,7 +30,6 @@
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 Down Expand Up @@ -65,7 +64,8 @@ public CalibrationInput(
private final Mat stdDeviationsIntrinsics = new Mat();
private final Mat stdDeviationsExtrinsics = new Mat();

// Contains the re projection error of each snapshot by re projecting the corners we found and
// Contains the re projection error of each snapshot by re projecting the
// corners we found and
// finding the Euclidean distance between the actual corners.
private final Mat perViewErrors = new Mat();

Expand Down Expand Up @@ -138,7 +138,8 @@ protected CameraCalibrationCoefficients calibrateOpenCV(
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 to calculate
// FindBoardCorners pipe outputs all the image points, object points, and frames
// to calculate
// imageSize from, other parameters are output Mats

Calib3d.calibrateCameraExtended(
Expand Down Expand Up @@ -190,7 +191,7 @@ protected CameraCalibrationCoefficients calibrateMrcal(
int imageHeight = (int) in.get(0).size.height;

MrCalResult result =
MrCalJNI.calibrateCamera(
MrCal.calibrateCamera(
corner_locations,
params.boardWidth,
params.boardHeight,
Expand Down Expand Up @@ -222,13 +223,36 @@ 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
// Calculate optimized board poses manually. We get this for free from mrcal
// too, but that's not
// JNIed (yet)
List<Mat> rvecs = new ArrayList<>();
List<Mat> tvecs = new ArrayList<>();

for (var o : in) {
var rvec = new Mat();
var tvec = new Mat();

Point3[] oPoints = o.objectPoints.toArray();
Point[] iPoints = o.imagePoints.toArray();

List<Point3> outputOPoints = new ArrayList<Point3>();
List<Point> outputIPoints = new ArrayList<Point>();

for (int i = 0; i < iPoints.length; i++) {
if (iPoints[i].x != -1 && iPoints[i].y != -1) {
outputIPoints.add(iPoints[i]);
}
}
for (int i = 0; i < oPoints.length; i++) {
if (oPoints[i].x != -1 && oPoints[i].y != -1 && oPoints[i].z != -1) {
outputOPoints.add(oPoints[i]);
}
}

o.objectPoints.fromList(outputOPoints);
o.imagePoints.fromList(outputIPoints);

Calib3d.solvePnP(
o.objectPoints,
o.imagePoints,
Expand Down Expand Up @@ -285,7 +309,8 @@ private List<BoardObservation> createObservations(
// Apply warp, if set
if (calobject_warp != null && calobject_warp.length == 2) {
// mrcal warp model!
// The chessboard spans [-1, 1] on the x and y axies. We then let z=k_x(1-x^2)+k_y(1-y^2)
// The chessboard spans [-1, 1] on the x and y axies. We then let
// z=k_x(1-x^2)+k_y(1-y^2)

double xmin = 0;
double ymin = 0;
Expand Down
119 changes: 56 additions & 63 deletions photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

package org.photonvision.vision.pipe.impl;

import java.util.Arrays;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
Expand All @@ -29,9 +30,12 @@
import org.photonvision.vision.pipeline.UICalibrationData;

public class FindBoardCornersPipe
extends
CVPipe<Pair<Mat, Mat>, FindBoardCornersPipe.FindBoardCornersPipeResult, FindBoardCornersPipe.FindCornersPipeParams> {
private static final Logger logger = new Logger(FindBoardCornersPipe.class, LogGroup.VisionModule);
extends CVPipe<
Pair<Mat, Mat>,
FindBoardCornersPipe.FindBoardCornersPipeResult,
FindBoardCornersPipe.FindCornersPipeParams> {
private static final Logger logger =
new Logger(FindBoardCornersPipe.class, LogGroup.VisionModule);

MatOfPoint3f objectPoints = new MatOfPoint3f();

Expand All @@ -42,10 +46,11 @@ public class FindBoardCornersPipe
// Since we return results in real-time, we want to ensure it goes as fast as
// possible
// and fails as fast as possible.
final int findChessboardFlags = Calib3d.CALIB_CB_NORMALIZE_IMAGE
| Calib3d.CALIB_CB_ADAPTIVE_THRESH
| Calib3d.CALIB_CB_FILTER_QUADS
| Calib3d.CALIB_CB_FAST_CHECK;
final int findChessboardFlags =
Calib3d.CALIB_CB_NORMALIZE_IMAGE
| Calib3d.CALIB_CB_ADAPTIVE_THRESH
| Calib3d.CALIB_CB_FILTER_QUADS
| Calib3d.CALIB_CB_FAST_CHECK;

private final MatOfPoint2f boardCorners = new MatOfPoint2f();

Expand All @@ -60,8 +65,7 @@ public class FindBoardCornersPipe
private FindCornersPipeParams lastParams = null;

public void createObjectPoints() {
if (this.lastParams != null && this.lastParams.equals(this.params))
return;
if (this.lastParams != null && this.lastParams.equals(this.params)) return;
this.lastParams = this.params;

this.objectPoints.release();
Expand All @@ -78,9 +82,10 @@ public void createObjectPoints() {
* squares, not the
* number of corners.
*/
this.patternSize = params.type == UICalibrationData.BoardType.CHESSBOARD
? new Size(params.boardWidth - 1, params.boardHeight - 1)
: new Size(params.boardWidth, params.boardHeight);
this.patternSize =
params.type == UICalibrationData.BoardType.CHESSBOARD
? new Size(params.boardWidth - 1, params.boardHeight - 1)
: new Size(params.boardWidth, params.boardHeight);

// Chessboard and dot board have different 3D points to project as a dot board
// has alternating
Expand All @@ -105,9 +110,7 @@ public void createObjectPoints() {
new Point3((2 * j + i % 2) * params.gridSize, i * params.gridSize, 0.0d)));
}
}
} else

{
} else {
logger.error("Can't create pattern for unknown board type " + params.type);
}
}
Expand All @@ -124,8 +127,7 @@ protected FindBoardCornersPipeResult process(Pair<Mat, Mat> in) {
}

/**
* Figures out how much a frame or point cloud must be scaled down by to match
* the desired size at
* Figures out how much a frame or point cloud must be scaled down by to match the desired size at
* which to run FindCorners. Should usually be > 1.
*
* @param inFrame
Expand All @@ -136,25 +138,16 @@ private double getFindCornersScaleFactor(Mat inFrame) {
}

/**
* Finds the minimum spacing between a set of x/y points Currently only
* considers points whose
* index is next to each other Which, currently, means it traverses one
* dimension. This is a rough
* Finds the minimum spacing between a set of x/y points Currently only considers points whose
* index is next to each other Which, currently, means it traverses one dimension. This is a rough
* heuristic approach which could be refined in the future.
*
* <p>
* Note that the current implementation can be fooled under the following
* conditions: (1) The
* width of the image is an odd number, and the smallest distance was actually
* on the between the
* last two points in a given row and (2) The smallest distance was actually in
* the direction
* orthogonal to that which was getting traversed by iterating through the
* MatOfPoint2f in order.
* <p>Note that the current implementation can be fooled under the following conditions: (1) The
* width of the image is an odd number, and the smallest distance was actually on the between the
* last two points in a given row and (2) The smallest distance was actually in the direction
* orthogonal to that which was getting traversed by iterating through the MatOfPoint2f in order.
*
* <p>
* I've chosen not to handle these for speed's sake, and because, really, you
* don't need the
* <p>I've chosen not to handle these for speed's sake, and because, really, you don't need the
* exact answer for "min distance". you just need something fairly reasonable.
*
* @param inPoints point set to analyze. Must be a "tall" matrix.
Expand All @@ -176,9 +169,8 @@ private double getApproxMinSpacing(MatOfPoint2f inPoints) {
}

/**
* @param inFrame Full-size mat that is going to get scaled down before passing
* to
* findBoardCorners
* @param inFrame Full-size mat that is going to get scaled down before passing to
* findBoardCorners
* @return the size to scale the input mat to
*/
private Size getFindCornersImgSize(Mat in) {
Expand All @@ -188,13 +180,11 @@ private Size getFindCornersImgSize(Mat in) {
}

/**
* Given an input frame and a set of points from the "smaller"
* findChessboardCorner analysis,
* Given an input frame and a set of points from the "smaller" findChessboardCorner analysis,
* re-scale the points back to where they would have been in the input frame
*
* @param inPoints set of points derived from a call to findChessboardCorner on
* a shrunken mat.
* Must be a "tall" matrix.
* @param inPoints set of points derived from a call to findChessboardCorner on a shrunken mat.
* Must be a "tall" matrix.
* @param origFrame Original frame we're rescaling points back to
* @param outPoints mat into which the output rescaled points get placed
*/
Expand All @@ -213,8 +203,7 @@ private void rescalePointsToOrigFrame(
}

/**
* Picks a window size for doing subpixel optimization based on the board type
* and spacing
* Picks a window size for doing subpixel optimization based on the board type and spacing
* observed between the corners or points in the image
*
* @param inPoints
Expand Down Expand Up @@ -258,17 +247,19 @@ private FindBoardCornersPipeResult findBoardCorners(Pair<Mat, Mat> in) {
}

// Run the chessboard corner finder on the smaller image
boardFound = Calib3d.findChessboardCorners(
smallerInFrame, patternSize, smallerBoardCorners, findChessboardFlags);
boardFound =
Calib3d.findChessboardCorners(
smallerInFrame, patternSize, smallerBoardCorners, findChessboardFlags);

// Rescale back to original pixel locations
if (boardFound) {
rescalePointsToOrigFrame(smallerBoardCorners, inFrame, boardCorners);
}

} else if (params.type == UICalibrationData.BoardType.DOTBOARD) {
boardFound = Calib3d.findCirclesGrid(
inFrame, patternSize, boardCorners, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
boardFound =
Calib3d.findCirclesGrid(
inFrame, patternSize, boardCorners, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
}
if (!boardFound) {
// If we can't find a chessboard/dot board, give up
Expand All @@ -293,7 +284,12 @@ private FindBoardCornersPipeResult findBoardCorners(Pair<Mat, Mat> in) {
// the corners we found
Calib3d.drawChessboardCorners(outFrame, patternSize, outBoardCorners, true);

return new FindBoardCornersPipeResult(inFrame.size(), objPts, outBoardCorners);
float[] levels = new float[(int) objPts.total()];
Arrays.fill(levels, 1.0f);
var outLevels = new MatOfFloat();
outLevels.fromArray(levels);

return new FindBoardCornersPipeResult(inFrame.size(), objPts, outBoardCorners, outLevels);
}

public static class FindCornersPipeParams {
Expand Down Expand Up @@ -338,19 +334,13 @@ public int hashCode() {

@Override
public boolean equals(Object obj) {
if (this == obj)
return true;
if (obj == null)
return false;
if (getClass() != obj.getClass())
return false;
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
FindCornersPipeParams other = (FindCornersPipeParams) obj;
if (boardHeight != other.boardHeight)
return false;
if (boardWidth != other.boardWidth)
return false;
if (type != other.type)
return false;
if (boardHeight != other.boardHeight) return false;
if (boardWidth != other.boardWidth) return false;
if (type != other.type) return false;
if (Double.doubleToLongBits(gridSize) != Double.doubleToLongBits(other.gridSize))
return false;
return divisor == other.divisor;
Expand All @@ -365,19 +355,22 @@ public static class FindBoardCornersPipeResult implements Releasable {
// Set later only if we need it
public Mat inputImage = null;

public MatOfFloat levels = null;

public FindBoardCornersPipeResult(
Size size, MatOfPoint3f objectPoints, MatOfPoint2f imagePoints) {
Size size, MatOfPoint3f objectPoints, MatOfPoint2f imagePoints, MatOfFloat levels) {
this.size = size;
this.objectPoints = objectPoints;
this.imagePoints = imagePoints;
this.levels = levels;
}

@Override
public void release() {
objectPoints.release();
imagePoints.release();
if (inputImage != null)
inputImage.release();
if (inputImage != null) inputImage.release();
if (levels != null) levels.release();
}
}
}
Loading

0 comments on commit ce00190

Please sign in to comment.