Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-28 02:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Show board outliers in calibration info card (#1267)

Browse Source
This commit is contained in:
Matt Morley
2025-12-26 21:20:36 -05:00
committed by GitHub
parent 235e601cbc
commit fddff5dbca
17 changed files with 1062 additions and 348 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java
View File

@@ -18,14 +18,23 @@
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.nio.file.Path;
import java.util.Arrays;
import java.util.List;
import org.jetbrains.annotations.Nullable;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import org.opencv.core.Scalar;
import org.opencv.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.util.ColorHelper;
// Ignore the previous calibration data that was stored in the json file.
@JsonIgnoreProperties(ignoreUnknown = true)
@@ -47,8 +56,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;
@@ -63,16 +72,22 @@ public final class BoardObservation implements Cloneable {
@JsonProperty("locationInImageSpace") List<Point> locationInImageSpace,
@JsonProperty("reprojectionErrors") List<Point> reprojectionErrors,
@JsonProperty("optimisedCameraToObject") Pose3d optimisedCameraToObject,
@JsonProperty("includeObservationInCalibration") boolean includeObservationInCalibration,
@JsonProperty("cornersUsed") boolean[] cornersUsed,
@JsonProperty("snapshotName") String snapshotName,
@JsonProperty("snapshotDataLocation") Path snapshotDataLocation) {
this.locationInObjectSpace = locationInObjectSpace;
this.locationInImageSpace = locationInImageSpace;
this.reprojectionErrors = reprojectionErrors;
this.optimisedCameraToObject = optimisedCameraToObject;
this.includeObservationInCalibration = includeObservationInCalibration;
this.snapshotName = snapshotName;
this.snapshotDataLocation = snapshotDataLocation;
// legacy migration -- we assume all points are inliers
if (cornersUsed == null) {
cornersUsed = new boolean[locationInObjectSpace.size()];
Arrays.fill(cornersUsed, true);
}
this.cornersUsed = cornersUsed;
}
@Override
@@ -85,8 +100,8 @@ public final class BoardObservation implements Cloneable {
+ reprojectionErrors
+ ", optimisedCameraToObject="
+ optimisedCameraToObject
+ ", includeObservationInCalibration="
+ includeObservationInCalibration
+ ", cornersUsed="
+ cornersUsed
+ ", snapshotName="
+ snapshotName
+ ", snapshotDataLocation="
@@ -103,4 +118,73 @@ public final class BoardObservation implements Cloneable {
return null;
}
}
@JsonIgnore
/**
* Load the captured board image from disk. Allocates a new Mat, which the caller is responsible
* for releasing.
*
* @return The loaded image, or null if it could not be loaded.
*/
public Mat loadImage() {
Mat img = Imgcodecs.imread(this.snapshotDataLocation.toString());
if (img == null || img.empty() || img.rows() == 0 || img.cols() == 0) {
return null;
}
return img;
}
/**
* Annotate the image with the detected corners, green for used, red for unused
*
* @return Annotated image, or null if the image could not be loaded. Caller is responsible for
* releasing the Mat.
*/
@JsonIgnore
public Mat annotateImage() {
var image = loadImage();
if (image == null) {
return null;
}
int thickness = Core.FILLED;
var diag = Math.hypot(image.width(), image.height());
int r = (int) Math.max(diag * 4.0 / 500.0, 3);
for (int i = 0; i < this.locationInImageSpace.size(); i++) {
var c = locationInImageSpace.get(i);
// -1, -1 means unused corner
if (c.x < 0 || c.y < 0) {
continue;
}
Scalar color;
if (cornersUsed[i]) {
color = ColorHelper.colorToScalar(Color.green);
} else {
color = ColorHelper.colorToScalar(Color.red);
}
Imgproc.circle(image, c, r, color, thickness);
}
return image;
}
/**
* Mean reprojection error for this observation, skipping corners marked as unused. The overall
* mean is calculated as the mean of each individual corner's reprojection error, or the distance
* in pixels between the observed and expected location.
*
* @return Mean reprojection error in pixels.
*/
@JsonIgnore
double meanReprojectionError() {
return reprojectionErrors.stream()
.filter(pt -> cornersUsed[reprojectionErrors.indexOf(pt)])
.mapToDouble(pt -> Math.hypot(pt.x, pt.y))
.average()
.orElse(0);
}
}

2
photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java
View File

@@ -217,7 +217,7 @@ public class CameraCalibrationCoefficients implements Releasable {
}
@JsonIgnore
public List<BoardObservation> getPerViewErrors() {
public List<BoardObservation> getObservations() {
return observations;
}

25
photon-core/src/main/java/org/photonvision/vision/calibration/UICameraCalibrationCoefficients.java
View File

@@ -18,13 +18,19 @@
package org.photonvision.vision.calibration;
import java.util.List;
import java.util.stream.IntStream;
import org.opencv.core.Size;
public class UICameraCalibrationCoefficients extends CameraCalibrationCoefficients {
public int numSnapshots;
/** Immutable list of mean errors. */
public List<Double> meanErrors;
public List<Integer> numMissing;
public List<Integer> numOutliers;
private static int countOutliers(BoardObservation obs) {
return (int) obs.locationInImageSpace.stream().filter(it -> it.x < 0 || it.y < 0).count();
}
public UICameraCalibrationCoefficients(
Size resolution,
@@ -47,14 +53,19 @@ public class UICameraCalibrationCoefficients extends CameraCalibrationCoefficien
lensmodel);
this.numSnapshots = observations.size();
this.meanErrors =
this.meanErrors = observations.stream().map(BoardObservation::meanReprojectionError).toList();
this.numOutliers =
observations.stream()
.map(
it2 ->
it2.reprojectionErrors.stream()
.mapToDouble(it -> Math.hypot(it.x, it.y))
.average()
.orElse(0))
obs ->
IntStream.range(0, obs.cornersUsed.length)
.filter(i -> !obs.cornersUsed[i])
.map(i -> 1)
.sum()
- countOutliers(obs))
.toList();
this.numMissing =
observations.stream().map(UICameraCalibrationCoefficients::countOutliers).toList();
}
}

106
photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
View File

@@ -22,7 +22,6 @@ import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.stream.Collectors;
import org.apache.commons.io.FileUtils;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
@@ -145,8 +144,8 @@ public class Calibrate3dPipe
}
// And delete rows depending on the level -- otherwise, level has no impact for opencv
List<Mat> objPoints = new ArrayList<>();
List<Mat> imgPoints = new ArrayList<>();
List<MatOfPoint3f> objPoints = new ArrayList<>();
List<MatOfPoint2f> imgPoints = new ArrayList<>();
for (int i = 0; i < objPointsIn.size(); i++) {
MatOfPoint3f objPtsOut = new MatOfPoint3f();
MatOfPoint2f imgPtsOut = new MatOfPoint2f();
@@ -174,8 +173,8 @@ public class Calibrate3dPipe
// imageSize from, other parameters are output Mats
Calib3d.calibrateCameraExtended(
objPoints,
imgPoints,
objPoints.stream().map(it -> (Mat) it).toList(),
imgPoints.stream().map(it -> (Mat) it).toList(),
new Size(in.get(0).size.width, in.get(0).size.height),
cameraMatrix,
distortionCoefficients,
@@ -194,9 +193,29 @@ public class Calibrate3dPipe
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;
})
.toList();
var observations =
createObservations(
in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null, imageSavePath);
in,
cameraMatrix,
distortionCoefficients,
rvecs,
tvecs,
inliners,
new double[] {0, 0},
objPoints,
imgPoints,
imageSavePath);
cameraMatrix.release();
distortionCoefficients.release();
@@ -266,11 +285,10 @@ public class Calibrate3dPipe
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)
// We get these from the JNI (retsult.optimizedPoses), but these are subtly different from the
// ones our code used to produce. To preserve consistency, continue to redo this math
List<Mat> rvecs = new ArrayList<>();
List<Mat> tvecs = new ArrayList<>();
for (var o : in) {
var rvec = new Mat();
var tvec = new Mat();
@@ -309,6 +327,8 @@ public class Calibrate3dPipe
tvecs.add(tvec);
}
List<MatOfPoint3f> objPoints = in.stream().map(it -> it.objectPoints).toList();
List<MatOfPoint2f> imgPts = in.stream().map(it -> it.imagePoints).toList();
List<BoardObservation> observations =
createObservations(
in,
@@ -316,7 +336,10 @@ public class Calibrate3dPipe
distortionCoefficientsMat.getAsMatOfDouble(),
rvecs,
tvecs,
result.cornersUsed,
new double[] {result.warp_x, result.warp_y},
objPoints,
imgPts,
imageSavePath);
rvecs.forEach(Mat::release);
@@ -339,13 +362,12 @@ public class Calibrate3dPipe
MatOfDouble distortionCoefficients_,
List<Mat> rvecs,
List<Mat> tvecs,
List<boolean[]> cornersUsed,
double[] calobject_warp,
List<MatOfPoint3f> objPoints,
List<MatOfPoint2f> imgPts,
Path imageSavePath) {
List<Mat> objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
List<Mat> imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
// Clear the calibration image folder of any old images before we save the new ones.
try {
FileUtils.cleanDirectory(imageSavePath.toFile());
} catch (Exception e) {
@@ -355,11 +377,24 @@ public class Calibrate3dPipe
// For each observation, calc reprojection error
Mat jac_temp = new Mat();
List<BoardObservation> observations = new ArrayList<>();
for (int i = 0; i < objPoints.size(); i++) {
for (int snapshotId = 0; snapshotId < objPoints.size(); snapshotId++) {
// Copy object points to a new mat to allow warp modification without affecting underlying
// data
MatOfPoint3f i_objPtsNative = new MatOfPoint3f();
objPoints.get(i).copyTo(i_objPtsNative);
var i_objPts = i_objPtsNative.toList();
var i_imgPts = ((MatOfPoint2f) imgPts.get(i)).toList();
objPoints.get(snapshotId).copyTo(i_objPtsNative);
List<Point> i_imgPts = imgPts.get(snapshotId).toList();
if (i_objPtsNative.rows() != i_imgPts.size()) {
throw new RuntimeException(
"Objpts size ("
+ i_objPtsNative.rows()
+ ") != imgpts size ("
+ i_imgPts.size()
+ ") for snapshot "
+ snapshotId
+ "!");
}
// Apply warp, if set
if (calobject_warp != null && calobject_warp.length == 2) {
@@ -384,12 +419,13 @@ public class Calibrate3dPipe
i_objPtsNative.fromArray(list);
}
// Project distorted object points to image space
var img_pts_reprojected = new MatOfPoint2f();
try {
Calib3d.projectPoints(
i_objPtsNative,
rvecs.get(i),
tvecs.get(i),
rvecs.get(snapshotId),
tvecs.get(snapshotId),
cameraMatrix_,
distortionCoefficients_,
img_pts_reprojected,
@@ -399,22 +435,38 @@ public class Calibrate3dPipe
e.printStackTrace();
continue;
}
var img_pts_reprojected_list = img_pts_reprojected.toList();
// Calculate reprojection error for each point
var reprojectionError = new ArrayList<Point>();
var img_pts_reprojected_list = img_pts_reprojected.toList();
for (int j = 0; j < img_pts_reprojected_list.size(); j++) {
// Outliers are not part of the calibration, so don't calculate error for them
if (!cornersUsed.get(snapshotId)[j]) {
continue;
}
// error = (measured - expected)
var measured = img_pts_reprojected_list.get(j);
var expected = i_imgPts.get(j);
// Sanity check -- negative corners make no sense here
if (!(measured.x >= 0 && measured.y >= 0 && expected.x >= 0 && expected.y >= 0)) {
throw new RuntimeException(
"Negative corner in reprojection error calc! Measured: "
+ measured
+ ", expected: "
+ expected);
}
var error = new Point(measured.x - expected.x, measured.y - expected.y);
reprojectionError.add(error);
}
var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(i), tvecs.get(i));
var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(snapshotId), tvecs.get(snapshotId));
var inputImage = in.get(i).inputImage;
var inputImage = in.get(snapshotId).inputImage;
Path image_path = null;
String snapshotName = "img" + i + ".png";
String snapshotName = "img" + snapshotId + ".png";
if (inputImage != null) {
image_path = Paths.get(imageSavePath.toString(), snapshotName);
Imgcodecs.imwrite(image_path.toString(), inputImage);
@@ -422,7 +474,13 @@ public class Calibrate3dPipe
observations.add(
new BoardObservation(
i_objPts, i_imgPts, reprojectionError, camToBoard, true, snapshotName, image_path));
i_objPtsNative.toList(),
i_imgPts,
reprojectionError,
camToBoard,
cornersUsed.get(snapshotId),
snapshotName,
image_path));
}
jac_temp.release();

28
photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
View File

@@ -287,7 +287,23 @@ public class FindBoardCornersPipe
imgPoints.copyTo(outBoardCorners);
objPoints.copyTo(objPts);
// Since ChaArUco can still detect without the whole board we need to send "fake" (all
// Mrcal wants our top-left corner at 0, 0. But charuco hands us the first corner at the first
// board intersection, which is inset a couple mm. Adjust such that the top-left corner is at
// 0,0
{
// don't trust any particular ordering
List<Point3> pointList = objPts.toList();
double minX = pointList.stream().mapToDouble(p -> p.x).min().orElse(0.0);
double minY = pointList.stream().mapToDouble(p -> p.y).min().orElse(0.0);
// Shift all object points so that the origin is at (0,0)
List<Point3> shiftedPoints =
pointList.stream().map(p -> new Point3(p.x - minX, p.y - minY, p.z)).toList();
objPts.fromList(shiftedPoints);
}
// Since ChArUco can still detect without the whole board we need to send "fake" (all
// values less than zero) points and then tell it to ignore that corner by setting the
// corresponding level to -1. Calibrate3dPipe deals with piping this into the correct format
// for each backend
@@ -298,12 +314,17 @@ public class FindBoardCornersPipe
new Point3[(this.params.boardHeight() - 1) * (this.params.boardWidth() - 1)];
levels = new float[(this.params.boardHeight() - 1) * (this.params.boardWidth() - 1)];
// cache
var outBoardCornersList = outBoardCorners.toList();
var outObjPtsList = objPts.toList();
for (int i = 0; i < detectedIds.total(); i++) {
int id = (int) detectedIds.get(i, 0)[0];
boardCorners[id] = outBoardCorners.toList().get(i);
objectPoints[id] = objPts.toList().get(i);
boardCorners[id] = outBoardCornersList.get(i);
objectPoints[id] = outObjPtsList.get(i);
levels[id] = 1.0f;
}
for (int i = 0; i < boardCorners.length; i++) {
if (boardCorners[i] == null) {
boardCorners[i] = new Point(-1, -1);
@@ -320,7 +341,6 @@ public class FindBoardCornersPipe
objPoints.release();
detectedCorners.release();
detectedIds.release();
} else { // If not ChArUco then do chessboard
// Reduce the image size to be much more manageable
// Note that opencv will copy the frame if no resize is requested; we can skip

12
photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
View File

@@ -279,6 +279,18 @@ public class Calibrate3dPipeTest {
System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics.toString());
System.out.println("Dist Coeffs: " + cal.distCoeffs.toString());
// calculate RMS error
double totalSquaredError = 0.0;
long totalPoints = 0;
for (var obs : cal.getObservations()) {
double sumErrorSq =
obs.reprojectionErrors.stream().mapToDouble(d -> d.x * d.x + d.y * d.y).sum();
totalSquaredError += sumErrorSq;
totalPoints += obs.reprojectionErrors.size();
}
double rmsError = Math.sqrt(totalSquaredError / totalPoints);
System.out.println("RMS Reprojection Error: " + rmsError);
// Confirm we didn't get leaky on our mat usage
// assertEquals(startMatCount, CVMat.getMatCount()); // TODO Figure out why this
// doesn't