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

[photonlib] Simulation robustness (#874)

Browse Source 
- `PNPResults` can now be empty (`isPresent` = false)
- solvePNP methods actually handle errors and return empty `PNPResults`
  - This reveals an odd error where some inputs to `solvePNP_SQUARE()` resulted in an estimated transform with NaN values, and attempts to handle it
- Overwrites java changes from #817 since #742 had duplicate fixes
- Minor bugfixes
This commit is contained in:
amquake
2023-07-23 18:32:36 -07:00
committed by GitHub
parent 454f8a1773
commit 816bbca2f1
8 changed files with 244 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -24,7 +24,6 @@
package org.photonvision;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Pair;
@@ -45,7 +44,6 @@ import java.util.Set;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
/**
* The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
@@ -364,44 +362,18 @@ public class PhotonPoseEstimator {
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
// Arrays we need declared up front
var visCorners = new ArrayList<TargetCorner>();
var knownVisTags = new ArrayList<AprilTag>();
if (result.getTargets().size() < 2) {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
if (!hasCalibData) {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}
for (var target : result.getTargets()) {
// Check if tag actually exists in the field layout
var tagPoseOpt = fieldTags.getTagPose(target.getFiducialId());
if (tagPoseOpt.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
// Now that we know tag is valid, add detected corners
visCorners.addAll(target.getDetectedCorners());
// actual layout poses of visible tags -- not exposed, so have to recreate
var tagPose = tagPoseOpt.get();
knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose));
}
if (result.getTargets().size() < 2 || knownVisTags.size() < 2) {
// Run fallback strategy instead
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData || result.getTargets().size() < 2) {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}
var pnpResults =
VisionEstimation.estimateCamPoseSqpnp(
cameraMatrixOpt.get(), distCoeffsOpt.get(), visCorners, knownVisTags);
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResults.isPresent)
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
var best =
new Pose3d()
.plus(pnpResults.best) // field-to-camera

232
photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View File

@@ -373,9 +373,10 @@ public final class OpenCVHelp {
}
/**
* Finds the transformation(s) that map the camera's pose to the target pose. The camera's pose
* Finds the transformation(s) that map the camera's pose to the target's pose. The camera's pose
* relative to the target is determined by the supplied 3d points of the target's model and their
* associated 2d points imaged by the camera.
* associated 2d points imaged by the camera. The supplied model translations must be relative to
* the target's pose.
*
* <p>For planar targets, there may be an alternate solution which is plausible given the 2d image
* points. This has an associated "ambiguity" which describes the ratio of reprojection error
@@ -384,8 +385,8 @@ public final class OpenCVHelp {
* <p>This method is intended for use with individual AprilTags, and will not work unless 4 points
* are provided.
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param modelTrls The translations of the object corners. These should have the object pose as
* their origin. These must come in a specific, pose-relative order (in NWU):
* <ul>
@@ -405,68 +406,102 @@ public final class OpenCVHelp {
Matrix<N5, N1> distCoeffs,
List<Translation3d> modelTrls,
List<TargetCorner> imageCorners) {
// IPPE_SQUARE expects our corners in a specific order
modelTrls = reorderCircular(modelTrls, true, -1);
imageCorners = reorderCircular(imageCorners, true, -1);
// translate to opencv classes
var objectPoints = translationToTvec(modelTrls.toArray(new Translation3d[0]));
var imagePoints = targetCornersToMat(imageCorners);
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
// solvepnp inputs
MatOfPoint3f objectPoints = new MatOfPoint3f();
MatOfPoint2f imagePoints = new MatOfPoint2f();
MatOfDouble cameraMatrixMat = new MatOfDouble();
MatOfDouble distCoeffsMat = new MatOfDouble();
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
var rvec = Mat.zeros(3, 1, CvType.CV_32F);
var tvec = Mat.zeros(3, 1, CvType.CV_32F);
var reprojectionError = new Mat();
// calc rvecs/tvecs and associated reprojection error from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_IPPE_SQUARE,
rvec,
tvec,
reprojectionError);
Mat rvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat tvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat reprojectionError = Mat.zeros(2, 1, CvType.CV_32F);
try {
// IPPE_SQUARE expects our corners in a specific order
modelTrls = reorderCircular(modelTrls, true, -1);
imageCorners = reorderCircular(imageCorners, true, -1);
// translate to opencv classes
translationToTvec(modelTrls.toArray(new Translation3d[0])).assignTo(objectPoints);
targetCornersToMat(imageCorners).assignTo(imagePoints);
matrixToMat(cameraMatrix.getStorage()).assignTo(cameraMatrixMat);
matrixToMat(distCoeffs.getStorage()).assignTo(distCoeffsMat);
float[] errors = new float[2];
reprojectionError.get(0, 0, errors);
// convert to wpilib coordinates
var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
float[] errors = new float[2];
Transform3d best = null;
Transform3d alt = null;
Transform3d alt = null;
if (tvecs.size() > 1) {
alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1)));
for (int tries = 0; tries < 2; tries++) {
// calc rvecs/tvecs and associated reprojection error from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_IPPE_SQUARE,
rvec,
tvec,
reprojectionError);
reprojectionError.get(0, 0, errors);
// convert to wpilib coordinates
best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
if (tvecs.size() > 1) {
alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1)));
}
// check if we got a NaN result
if (!Double.isNaN(errors[0])) break;
else { // add noise and retry
double[] br = imagePoints.get(0, 0);
br[0] -= 0.001;
br[1] -= 0.001;
imagePoints.put(0, 0, br);
}
}
// check if solvePnP failed with NaN results and retrying failed
if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result");
if (alt != null)
return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResults(best, errors[0]);
}
// solvePnP failed
catch (Exception e) {
System.err.println("SolvePNP_SQUARE failed!");
e.printStackTrace();
return new PNPResults();
} finally {
// release our Mats from native memory
objectPoints.release();
imagePoints.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();
}
// release our Mats from native memory
objectPoints.release();
imagePoints.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();
if (alt != null) return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResults(best, errors[0]);
}
/**
* Finds the transformation that maps the camera's pose to the target pose. The camera's pose
* relative to the target is determined by the supplied 3d points of the target's model and their
* associated 2d points imaged by the camera.
* Finds the transformation that maps the camera's pose to the origin of the supplied object. An
* "object" is simply a set of known 3d translations that correspond to the given 2d points. If,
* for example, the object translations are given relative to close-right corner of the blue
* alliance(the default origin), a camera-to-origin transformation is returned. If the
* translations are relative to a target's pose, a camera-to-target transformation is returned.
*
* <p>This method is intended for use with multiple targets and has no alternate solutions. There
* must be at least 3 points.
* <p>There must be at least 3 points to use this method. This does not return an alternate
* solution-- if you are intending to use solvePNP on a single AprilTag, see {@link
* #solvePNP_SQUARE} instead.
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param objectTrls The translations of the object corners, relative to the field.
* @param imageCorners The projection of these 3d object points into the 2d camera image. The
* order should match the given object point translations.
@@ -479,46 +514,55 @@ public final class OpenCVHelp {
Matrix<N5, N1> distCoeffs,
List<Translation3d> objectTrls,
List<TargetCorner> imageCorners) {
// translate to opencv classes
var objectPoints = translationToTvec(objectTrls.toArray(new Translation3d[0]));
var imagePoints = targetCornersToMat(imageCorners);
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
var rvec = Mat.zeros(3, 1, CvType.CV_32F);
var tvec = Mat.zeros(3, 1, CvType.CV_32F);
var reprojectionError = new Mat();
// calc rvec/tvec from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_SQPNP,
rvec,
tvec,
reprojectionError);
try {
// translate to opencv classes
MatOfPoint3f objectPoints = translationToTvec(objectTrls.toArray(new Translation3d[0]));
MatOfPoint2f imagePoints = targetCornersToMat(imageCorners);
MatOfDouble cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
MatOfDouble distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
Mat rvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat tvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat reprojectionError = new Mat();
// calc rvec/tvec from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_SQPNP,
rvec,
tvec,
reprojectionError);
float[] error = new float[1];
reprojectionError.get(0, 0, error);
// convert to wpilib coordinates
var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
float[] error = new float[1];
reprojectionError.get(0, 0, error);
// convert to wpilib coordinates
var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
// release our Mats from native memory
objectPoints.release();
imagePoints.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();
// release our Mats from native memory
objectPoints.release();
imagePoints.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();
return new PNPResults(best, error[0]);
// check if solvePnP failed with NaN results
if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result");
return new PNPResults(best, error[0]);
} catch (Exception e) {
System.err.println("SolvePNP_SQPNP failed!");
e.printStackTrace();
return new PNPResults();
}
}
}

21
photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java
View File

@@ -36,7 +36,19 @@ import edu.wpi.first.math.geometry.Transform3d;
* method.
*/
public class PNPResults {
/**
* If this result is valid. A false value indicates there was an error in estimation, and this
* result should not be used.
*/
public final boolean isPresent;
/**
* The best-fit transform. The coordinate frame of this transform depends on the method which gave
* this result.
*/
public final Transform3d best;
/** Reprojection error of the best solution, in pixels */
public final double bestReprojErr;
/**
@@ -51,8 +63,14 @@ public class PNPResults {
/** If no alternate solution is found, this is 0 */
public final double ambiguity;
/** An empty (invalid) result. */
public PNPResults() {
this(new Transform3d(), new Transform3d(), 0, 0, 0);
this.isPresent = false;
this.best = new Transform3d();
this.alt = new Transform3d();
this.ambiguity = 0;
this.bestReprojErr = 0;
this.altReprojErr = 0;
}
public PNPResults(Transform3d best, double bestReprojErr) {
@@ -65,6 +83,7 @@ public class PNPResults {
double ambiguity,
double bestReprojErr,
double altReprojErr) {
this.isPresent = true;
this.best = best;
this.alt = alt;
this.ambiguity = ambiguity;

87
photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java
View File

@@ -31,7 +31,6 @@ import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
@@ -40,7 +39,7 @@ import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
public class VisionEstimation {
/** Get the visible {@link AprilTag}s according the tag layout using the visible tag IDs. */
/** Get the visible {@link AprilTag}s which are in the tag layout using the visible tag IDs. */
public static List<AprilTag> getVisibleLayoutTags(
List<PhotonTrackedTarget> visTags, AprilTagFieldLayout tagLayout) {
return visTags.stream()
@@ -55,19 +54,23 @@ public class VisionEstimation {
}
/**
* Performs solvePNP using 3d-2d point correspondences to estimate the field-to-camera
* transformation. If only one tag is visible, the result may have an alternate solution.
* Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the
* field-to-camera transformation. If only one tag is visible, the result may have an alternate
* solution.
*
* <p><b>Note:</b> The returned transformation is from the field origin to the camera pose!
* (Unless you only feed this one tag??)
*
* <p>With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}
*
* <p>With multiple tags: {@link OpenCVHelp#solvePNP_SQPNP}
*
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param visTags The visible tags reported by PV
* @param tagLayout The known tag layout on the field
* @return The transformation that maps the field origin to the camera pose
* @return The transformation that maps the field origin to the camera pose. Ensure the {@link
* PNPResults} are present before utilizing them.
*/
@Deprecated
public static PNPResults estimateCamPosePNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
@@ -92,24 +95,12 @@ public class VisionEstimation {
var camToTag =
OpenCVHelp.solvePNP_SQUARE(
cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners);
if (!camToTag.isPresent) return new PNPResults();
var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
var altPose = new Pose3d();
if (camToTag.ambiguity != 0)
altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse());
var bestTagToCam = camToTag.best.inverse();
SmartDashboard.putNumberArray(
"multiTagBest_internal",
new double[] {
bestTagToCam.getX(),
bestTagToCam.getY(),
bestTagToCam.getZ(),
bestTagToCam.getRotation().getQuaternion().getW(),
bestTagToCam.getRotation().getQuaternion().getX(),
bestTagToCam.getRotation().getQuaternion().getY(),
bestTagToCam.getRotation().getQuaternion().getZ()
});
var o = new Pose3d();
return new PNPResults(
new Transform3d(o, bestPose),
@@ -123,6 +114,7 @@ public class VisionEstimation {
var objectTrls = new ArrayList<Translation3d>();
for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose));
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners);
if (!camToOrigin.isPresent) return new PNPResults();
return new PNPResults(
camToOrigin.best.inverse(),
camToOrigin.alt.inverse(),
@@ -131,59 +123,4 @@ public class VisionEstimation {
camToOrigin.altReprojErr);
}
}
/**
* Performs solvePNP using 3d-2d point correspondences to estimate the field-to-camera
* transformation. If only one tag is visible, the result may have an alternate solution.
*
* <p><b>Note:</b> The returned transformation is from the field origin to the camera pose!
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param corners The visible tag corners in the 2d image
* @param knownTags The known tag field poses corresponding to the visible tag IDs
* @return The transformation that maps the field origin to the camera pose
*/
public static PNPResults estimateCamPoseSqpnp(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<TargetCorner> corners,
List<AprilTag> knownTags) {
if (knownTags == null
|| corners == null
|| corners.size() != knownTags.size() * 4
|| knownTags.size() == 0) {
return new PNPResults();
}
var objectTrls = new ArrayList<Translation3d>();
for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.vertices);
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners);
// var camToOrigin = OpenCVHelp.solveTagsPNPRansac(prop, objectTrls, corners);
return new PNPResults(
camToOrigin.best.inverse(),
camToOrigin.alt.inverse(),
camToOrigin.ambiguity,
camToOrigin.bestReprojErr,
camToOrigin.altReprojErr);
}
/**
* The best estimated transformation (Rotation-translation composition) that maps a set of
* translations onto another with point correspondences, and its RMSE.
*/
public static class SVDResults {
public final RotTrlTransform3d trf;
/** If the result is invalid, this value is -1 */
public final double rmse;
public SVDResults() {
this(new RotTrlTransform3d(), -1);
}
public SVDResults(RotTrlTransform3d trf, double rmse) {
this.trf = trf;
this.rmse = rmse;
}
}
}

5
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -70,7 +70,7 @@ public class PhotonCameraSim implements AutoCloseable {
private long nextNTEntryTime = WPIUtilJNI.now();
private double maxSightRangeMeters = Double.MAX_VALUE;
private static final double kDefaultMinAreaPx = 90;
private static final double kDefaultMinAreaPx = 100;
private double minTargetAreaPercent;
private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
@@ -356,6 +356,7 @@ public class PhotonCameraSim implements AutoCloseable {
prop.getDistCoeffs(),
tgt.getModel().vertices,
noisyTargetCorners);
if (!pnpSim.isPresent) continue;
centerRot =
prop.getPixelRot(
OpenCVHelp.projectPoints(
@@ -459,6 +460,8 @@ public class PhotonCameraSim implements AutoCloseable {
ts.targetPoseEntry.set(poseData, receiveTimestamp);
}
ts.cameraIntrinsicsPublisher.set(prop.getIntrinsics().getData(), receiveTimestamp);
ts.cameraDistortionPublisher.set(prop.getDistCoeffs().getData(), receiveTimestamp);
ts.heartbeatPublisher.set(heartbeatCounter++, receiveTimestamp);
}
}

13
photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java
View File

@@ -109,19 +109,10 @@ public class VideoSimUtil {
name = name.substring(0, name.length() - idString.length()) + idString;
var resource = VideoSimUtil.class.getResource(kResourceTagImagesPath + name + ".png");
// local IDE tests
String path = kLocalTagImagesPath + name + ".png";
// gradle tests
if (resource != null) {
path = resource.getPath();
// TODO why did we have this previously?
// if (path.startsWith("/")) path = path.substring(1);
}
Mat result = new Mat();
if (!path.startsWith("file")) result = Imgcodecs.imread(path, Imgcodecs.IMREAD_GRAYSCALE);
// reading jar file
if (result.empty()) {
if (resource != null && resource.getPath().startsWith("file")) {
BufferedImage buf;
try {
buf = ImageIO.read(resource);
@@ -140,6 +131,8 @@ public class VideoSimUtil {
}
}
}
// local IDE tests
else result = Imgcodecs.imread(kLocalTagImagesPath + name + ".png", Imgcodecs.IMREAD_GRAYSCALE);
return result;
}

2
photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View File

@@ -355,7 +355,7 @@ public class VisionSystemSim {
// use camera pose from the image capture timestamp
Pose3d lateRobotPose = getRobotPose(timestampCapture);
Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim).get());
Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim, timestampCapture).get());
cameraPose2ds.add(lateCameraPose.toPose2d());
// process a PhotonPipelineResult with visible targets