From 4c2aa16c1c3512d0f99b0df9516d2d1f10fe5f9a Mon Sep 17 00:00:00 2001 From: amquake Date: Mon, 19 Jun 2023 21:00:27 -0700 Subject: [PATCH 01/13] optional pnp results, pnp method simplification --- .../org/photonvision/PhotonPoseEstimator.java | 41 +--- .../photonvision/estimation/OpenCVHelp.java | 209 ++++++++++-------- .../estimation/VisionEstimation.java | 101 ++------- .../simulation/PhotonCameraSim.java | 3 +- .../simulation/VisionSystemSim.java | 2 +- .../java/org/photonvision/OpenCVTest.java | 4 +- 6 files changed, 146 insertions(+), 214 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 3eecf72118..56537e5f0b 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -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 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,17 @@ private Optional multiTagPNPStrategy( PhotonPipelineResult result, Optional> cameraMatrixOpt, Optional> distCoeffsOpt) { - // Arrays we need declared up front - var visCorners = new ArrayList(); - var knownVisTags = new ArrayList(); - - 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).orElse(null); + // try fallback strategy if solvePNP fails for some reason + if(pnpResults == null) return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); var best = new Pose3d() .plus(pnpResults.best) // field-to-camera diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java index b834ad4459..5ec848f424 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -38,6 +38,8 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.List; +import java.util.Optional; + import org.ejml.simple.SimpleMatrix; import org.opencv.calib3d.Calib3d; import org.opencv.core.Core; @@ -398,63 +400,74 @@ public static double getContourAreaPx(List corners) { * @param imageCorners The projection of these 3d object points into the 2d camera image. The * order should match the given object point translations. * @return The resulting transformation that maps the camera pose to the target pose and the - * ambiguity if an alternate solution is available. + * ambiguity if an alternate solution is available, or an empty Optional if solvePNP fails. */ - public static PNPResults solvePNP_SQUARE( + public static Optional solvePNP_SQUARE( Matrix cameraMatrix, Matrix distCoeffs, List modelTrls, List 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()); - var rvecs = new ArrayList(); - var tvecs = new ArrayList(); - 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); - - 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))); - - Transform3d alt = null; - if (tvecs.size() > 1) { - alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1))); + try { + // 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()); + var rvecs = new ArrayList(); + var tvecs = new ArrayList(); + 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); + + 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))); + + Transform3d alt = null; + if (tvecs.size() > 1) { + alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1))); + } + + // 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(); + + // check if solvePnP failed with NaN results + if(Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result"); + + if (alt != null) return Optional.of(new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1])); + else return Optional.of(new PNPResults(best, errors[0])); + } + // solvePnP failed + catch(Exception e) { + System.err.println("SolvePNP_SQUARE failed!"); + e.printStackTrace(); + return Optional.empty(); } - - // 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]); } /** @@ -474,51 +487,61 @@ public static PNPResults solvePNP_SQUARE( * model points are supplied relative to the origin, this transformation brings the camera to * the origin. */ - public static PNPResults solvePNP_SQPNP( + public static Optional solvePNP_SQPNP( Matrix cameraMatrix, Matrix distCoeffs, List objectTrls, List 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(); - var tvecs = new ArrayList(); - 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); - - 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(); - - return new PNPResults(best, error[0]); + try { + // 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(); + var tvecs = new ArrayList(); + 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); + + 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(); + + // check if solvePnP failed with NaN results + if(Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result"); + + return Optional.of(new PNPResults(best, error[0])); + } + catch (Exception e) { + System.err.println("SolvePNP_SQPNP failed!"); + e.printStackTrace(); + return Optional.empty(); + } } } diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java index 9cdc1811fb..30756eaa44 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -35,12 +35,13 @@ import java.util.ArrayList; import java.util.List; import java.util.Objects; +import java.util.Optional; import java.util.stream.Collectors; 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 getVisibleLayoutTags( List visTags, AprilTagFieldLayout tagLayout) { return visTags.stream() @@ -55,20 +56,22 @@ public static List getVisibleLayoutTags( } /** - * Performs solvePNP using 3d-2d point correspondences to estimate the field-to-camera + * 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. * *

Note: The returned transformation is from the field origin to the camera pose! - * (Unless you only feed this one tag??) + * + *

With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}

+ *

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, or an empty + * Optional if estimation fails. */ - @Deprecated - public static PNPResults estimateCamPosePNP( + public static Optional estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, List visTags, @@ -77,113 +80,47 @@ public static PNPResults estimateCamPosePNP( || visTags == null || tagLayout.getTags().size() == 0 || visTags.size() == 0) { - return new PNPResults(); + return Optional.empty(); } var corners = new ArrayList(); for (var tag : visTags) corners.addAll(tag.getDetectedCorners()); var knownTags = getVisibleLayoutTags(visTags, tagLayout); if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { - return new PNPResults(); + return Optional.empty(); } // single-tag pnp if (visTags.size() == 1) { var camToTag = OpenCVHelp.solvePNP_SQUARE( - cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners); + cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners).orElse(null); + if(camToTag == null) return Optional.empty(); 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( + return Optional.of(new PNPResults( new Transform3d(o, bestPose), new Transform3d(o, altPose), camToTag.ambiguity, camToTag.bestReprojErr, - camToTag.altReprojErr); + camToTag.altReprojErr)); } // multi-tag pnp else { var objectTrls = new ArrayList(); for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose)); - var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners); - return new PNPResults( + var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners).orElse(null); + if(camToOrigin == null) return Optional.empty(); + return Optional.of(new PNPResults( camToOrigin.best.inverse(), camToOrigin.alt.inverse(), camToOrigin.ambiguity, camToOrigin.bestReprojErr, - 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. - * - *

Note: 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 cameraMatrix, - Matrix distCoeffs, - List corners, - List knownTags) { - if (knownTags == null - || corners == null - || corners.size() != knownTags.size() * 4 - || knownTags.size() == 0) { - return new PNPResults(); - } - var objectTrls = new ArrayList(); - 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; + camToOrigin.altReprojErr)); } } } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 710d7f49bf..115b6c1015 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -355,7 +355,8 @@ public PhotonPipelineResult process( prop.getIntrinsics(), prop.getDistCoeffs(), tgt.getModel().vertices, - noisyTargetCorners); + noisyTargetCorners).orElse(null); + if(pnpSim == null) continue; centerRot = prop.getPixelRot( OpenCVHelp.projectPoints( diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index 6a1c36cf3d..81736d2dde 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -355,7 +355,7 @@ public void update(Pose3d robotPoseMeters) { // 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 diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java index 594439e847..baa57bdc6d 100644 --- a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java +++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java @@ -194,7 +194,7 @@ public void testSolvePNP_SQUARE() { prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); var pnpSim = OpenCVHelp.solvePNP_SQUARE( - prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners); + prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners).orElseThrow(); var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); assertSame(actualRelation.camToTarg, estRelation.camToTarg); } @@ -222,7 +222,7 @@ public void testSolvePNP_SQPNP() { prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); var pnpSim = OpenCVHelp.solvePNP_SQPNP( - prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners); + prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners).orElseThrow(); var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); assertSame(actualRelation.camToTarg, estRelation.camToTarg); } From 209e184098d931d5ffbbadf879a32f4ef10a0fd6 Mon Sep 17 00:00:00 2001 From: amquake Date: Tue, 20 Jun 2023 00:57:16 -0700 Subject: [PATCH 02/13] solvepnp docs clarification --- .../photonvision/estimation/OpenCVHelp.java | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 5ec848f424..28968f3630 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -375,9 +375,10 @@ public static double getContourAreaPx(List corners) { } /** - * 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. * *

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 @@ -386,8 +387,8 @@ public static double getContourAreaPx(List corners) { *

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): *

    @@ -471,15 +472,17 @@ public static Optional solvePNP_SQUARE( } /** - * 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. * - *

    This method is intended for use with multiple targets and has no alternate solutions. There - * must be at least 3 points. + *

    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. From b4a0c902f7a69cfb90398c164b19e6604319124f Mon Sep 17 00:00:00 2001 From: amquake Date: Tue, 20 Jun 2023 01:34:59 -0700 Subject: [PATCH 03/13] spotless --- .../org/photonvision/PhotonPoseEstimator.java | 6 ++- .../photonvision/estimation/OpenCVHelp.java | 28 +++++----- .../estimation/VisionEstimation.java | 53 ++++++++++--------- .../simulation/PhotonCameraSim.java | 11 ++-- .../java/org/photonvision/OpenCVTest.java | 12 ++++- 5 files changed, 63 insertions(+), 47 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 56537e5f0b..a669ab0b00 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -370,9 +370,11 @@ private Optional multiTagPNPStrategy( var pnpResults = VisionEstimation.estimateCamPosePNP( - cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags).orElse(null); + cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags) + .orElse(null); // try fallback strategy if solvePNP fails for some reason - if(pnpResults == null) return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); + if (pnpResults == null) + return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); var best = new Pose3d() .plus(pnpResults.best) // field-to-camera diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 28968f3630..87e2919b68 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -39,7 +39,6 @@ import java.util.Arrays; import java.util.List; import java.util.Optional; - import org.ejml.simple.SimpleMatrix; import org.opencv.calib3d.Calib3d; import org.opencv.core.Core; @@ -458,13 +457,14 @@ public static Optional solvePNP_SQUARE( reprojectionError.release(); // check if solvePnP failed with NaN results - if(Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result"); + if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result"); - if (alt != null) return Optional.of(new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1])); + if (alt != null) + return Optional.of(new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1])); else return Optional.of(new PNPResults(best, errors[0])); } // solvePnP failed - catch(Exception e) { + catch (Exception e) { System.err.println("SolvePNP_SQUARE failed!"); e.printStackTrace(); return Optional.empty(); @@ -472,14 +472,15 @@ public static Optional solvePNP_SQUARE( } /** - * 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. + * 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. * - *

    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. + *

    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 @@ -537,11 +538,10 @@ public static Optional solvePNP_SQPNP( reprojectionError.release(); // check if solvePnP failed with NaN results - if(Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result"); + if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result"); return Optional.of(new PNPResults(best, error[0])); - } - catch (Exception e) { + } catch (Exception e) { System.err.println("SolvePNP_SQPNP failed!"); e.printStackTrace(); return Optional.empty(); diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java index 30756eaa44..3df96f0642 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -31,7 +31,6 @@ 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; @@ -56,20 +55,22 @@ public static List getVisibleLayoutTags( } /** - * 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. + * 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. * *

    Note: The returned transformation is from the field origin to the camera pose! - * - *

    With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}

    - *

    With multiple tags: {@link OpenCVHelp#solvePNP_SQPNP}

    + * + *

    With only one tag: {@link OpenCVHelp#solvePNP_SQUARE} + * + *

    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, or an empty - * Optional if estimation fails. + * @return The transformation that maps the field origin to the camera pose, or an empty Optional + * if estimation fails. */ public static Optional estimateCamPosePNP( Matrix cameraMatrix, @@ -94,33 +95,37 @@ public static Optional estimateCamPosePNP( if (visTags.size() == 1) { var camToTag = OpenCVHelp.solvePNP_SQUARE( - cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners).orElse(null); - if(camToTag == null) return Optional.empty(); + cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners) + .orElse(null); + if (camToTag == null) return Optional.empty(); 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 o = new Pose3d(); - return Optional.of(new PNPResults( - new Transform3d(o, bestPose), - new Transform3d(o, altPose), - camToTag.ambiguity, - camToTag.bestReprojErr, - camToTag.altReprojErr)); + return Optional.of( + new PNPResults( + new Transform3d(o, bestPose), + new Transform3d(o, altPose), + camToTag.ambiguity, + camToTag.bestReprojErr, + camToTag.altReprojErr)); } // multi-tag pnp else { var objectTrls = new ArrayList(); for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose)); - var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners).orElse(null); - if(camToOrigin == null) return Optional.empty(); - return Optional.of(new PNPResults( - camToOrigin.best.inverse(), - camToOrigin.alt.inverse(), - camToOrigin.ambiguity, - camToOrigin.bestReprojErr, - camToOrigin.altReprojErr)); + var camToOrigin = + OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners).orElse(null); + if (camToOrigin == null) return Optional.empty(); + return Optional.of( + new PNPResults( + camToOrigin.best.inverse(), + camToOrigin.alt.inverse(), + camToOrigin.ambiguity, + camToOrigin.bestReprojErr, + camToOrigin.altReprojErr)); } } } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 115b6c1015..ad23dd8127 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -352,11 +352,12 @@ public PhotonPipelineResult process( if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP pnpSim = OpenCVHelp.solvePNP_SQUARE( - prop.getIntrinsics(), - prop.getDistCoeffs(), - tgt.getModel().vertices, - noisyTargetCorners).orElse(null); - if(pnpSim == null) continue; + prop.getIntrinsics(), + prop.getDistCoeffs(), + tgt.getModel().vertices, + noisyTargetCorners) + .orElse(null); + if (pnpSim == null) continue; centerRot = prop.getPixelRot( OpenCVHelp.projectPoints( diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java index baa57bdc6d..ae52dde639 100644 --- a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java +++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java @@ -194,7 +194,11 @@ public void testSolvePNP_SQUARE() { prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); var pnpSim = OpenCVHelp.solvePNP_SQUARE( - prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners).orElseThrow(); + prop.getIntrinsics(), + prop.getDistCoeffs(), + target.getModel().vertices, + targetCorners) + .orElseThrow(); var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); assertSame(actualRelation.camToTarg, estRelation.camToTarg); } @@ -222,7 +226,11 @@ public void testSolvePNP_SQPNP() { prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); var pnpSim = OpenCVHelp.solvePNP_SQPNP( - prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners).orElseThrow(); + prop.getIntrinsics(), + prop.getDistCoeffs(), + target.getModel().vertices, + targetCorners) + .orElseThrow(); var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); assertSame(actualRelation.camToTarg, estRelation.camToTarg); } From 1511b2aef545e6a6d30e56c6ffc77cc27d83bf02 Mon Sep 17 00:00:00 2001 From: amquake Date: Tue, 27 Jun 2023 00:58:20 -0700 Subject: [PATCH 04/13] fix local tag image loading --- .../photonvision/simulation/VideoSimUtil.java | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index cec341b4d8..63170a0260 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -109,19 +109,10 @@ public static Mat get16h5TagImage(int id) { 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); + + Mat result = new Mat(); // 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 static Mat get16h5TagImage(int id) { } } } + // local IDE tests + else result = Imgcodecs.imread(kLocalTagImagesPath + name + ".png", Imgcodecs.IMREAD_GRAYSCALE); return result; } From 5be455712d71541d8d9244eabe48f9c847f91e86 Mon Sep 17 00:00:00 2001 From: amquake Date: Sat, 1 Jul 2023 00:11:03 -0700 Subject: [PATCH 05/13] solvepnp square NaN retry --- .../photonvision/estimation/OpenCVHelp.java | 114 ++++++++++-------- .../org/photonvision/VisionSystemSimTest.java | 4 +- 2 files changed, 69 insertions(+), 49 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 87e2919b68..78dda17e12 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -407,56 +407,64 @@ public static Optional solvePNP_SQUARE( Matrix distCoeffs, List modelTrls, List imageCorners) { + // solvepnp inputs + MatOfPoint3f objectPoints = new MatOfPoint3f(); + MatOfPoint2f imagePoints = new MatOfPoint2f(); + MatOfDouble cameraMatrixMat = new MatOfDouble(); + MatOfDouble distCoeffsMat = new MatOfDouble(); + var rvecs = new ArrayList(); + var tvecs = new ArrayList(); + 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 - var objectPoints = translationToTvec(modelTrls.toArray(new Translation3d[0])); - var imagePoints = targetCornersToMat(imageCorners); - var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage()); - var distCoeffsMat = matrixToMat(distCoeffs.getStorage()); - var rvecs = new ArrayList(); - var tvecs = new ArrayList(); - 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); + 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))); - + Transform3d best = null; Transform3d alt = null; - if (tvecs.size() > 1) { - alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1))); - } - // 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(); + 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 + // 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) @@ -469,6 +477,18 @@ public static Optional solvePNP_SQUARE( e.printStackTrace(); return Optional.empty(); } + 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(); + } } /** @@ -498,15 +518,15 @@ public static Optional solvePNP_SQPNP( List imageCorners) { try { // 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()); + 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(); var tvecs = new ArrayList(); - var rvec = Mat.zeros(3, 1, CvType.CV_32F); - var tvec = Mat.zeros(3, 1, CvType.CV_32F); - var reprojectionError = new 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, diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index f4d5806e43..77c6859b8b 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -144,7 +144,7 @@ public void testVisibilityCupidShuffle() { var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); - visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 3.0), 3)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 1.0), 3)); // To the right, to the right var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70)); @@ -439,7 +439,7 @@ public void testMultipleTargets() { 5)); visionSysSim.addVisionTargets( new VisionTargetSim( - targetPoseL.transformBy( + targetPoseR.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), TargetModel.kTag16h5, 6)); From c9d2f1e0b2495bdd03d8f9ba5bff11fbce4cbddf Mon Sep 17 00:00:00 2001 From: amquake Date: Sat, 1 Jul 2023 00:12:37 -0700 Subject: [PATCH 06/13] spotless --- .../java/org/photonvision/estimation/OpenCVHelp.java | 11 +++++------ .../org/photonvision/simulation/VideoSimUtil.java | 4 ++-- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 78dda17e12..ec34afb1e9 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -431,7 +431,7 @@ public static Optional solvePNP_SQUARE( Transform3d best = null; Transform3d alt = null; - for(int tries = 0; tries < 2; tries++) { + for (int tries = 0; tries < 2; tries++) { // calc rvecs/tvecs and associated reprojection error from image points Calib3d.solvePnPGeneric( objectPoints, @@ -455,11 +455,11 @@ public static Optional solvePNP_SQUARE( } // check if we got a NaN result - if(!Double.isNaN(errors[0])) break; + 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; + br[0] -= 0.001; + br[1] -= 0.001; imagePoints.put(0, 0, br); } } @@ -476,8 +476,7 @@ public static Optional solvePNP_SQUARE( System.err.println("SolvePNP_SQUARE failed!"); e.printStackTrace(); return Optional.empty(); - } - finally { + } finally { // release our Mats from native memory objectPoints.release(); imagePoints.release(); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index 63170a0260..d142347a9a 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -109,8 +109,8 @@ public static Mat get16h5TagImage(int id) { name = name.substring(0, name.length() - idString.length()) + idString; var resource = VideoSimUtil.class.getResource(kResourceTagImagesPath + name + ".png"); - - Mat result = new Mat(); + + Mat result = new Mat(); // reading jar file if (resource != null && resource.getPath().startsWith("file")) { BufferedImage buf; From 6b22823e28da1ba066b4092c59dc2863ab1358db Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 23 Jul 2023 00:48:54 -0700 Subject: [PATCH 07/13] use condition in result instead of Optional --- .../org/photonvision/PhotonPoseEstimator.java | 5 +- .../photonvision/estimation/OpenCVHelp.java | 17 +++---- .../photonvision/estimation/PNPResults.java | 13 ++++- .../estimation/VisionEstimation.java | 50 ++++++++----------- .../simulation/PhotonCameraSim.java | 5 +- .../java/org/photonvision/OpenCVTest.java | 6 +-- 6 files changed, 48 insertions(+), 48 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index a669ab0b00..1e2db3d24a 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -370,10 +370,9 @@ private Optional multiTagPNPStrategy( var pnpResults = VisionEstimation.estimateCamPosePNP( - cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags) - .orElse(null); + cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags); // try fallback strategy if solvePNP fails for some reason - if (pnpResults == null) + if (!pnpResults.isPresent) return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); var best = new Pose3d() diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java index ec34afb1e9..944bc0708a 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -38,7 +38,6 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.List; -import java.util.Optional; import org.ejml.simple.SimpleMatrix; import org.opencv.calib3d.Calib3d; import org.opencv.core.Core; @@ -400,9 +399,9 @@ public static double getContourAreaPx(List corners) { * @param imageCorners The projection of these 3d object points into the 2d camera image. The * order should match the given object point translations. * @return The resulting transformation that maps the camera pose to the target pose and the - * ambiguity if an alternate solution is available, or an empty Optional if solvePNP fails. + * ambiguity if an alternate solution is available. */ - public static Optional solvePNP_SQUARE( + public static PNPResults solvePNP_SQUARE( Matrix cameraMatrix, Matrix distCoeffs, List modelTrls, @@ -468,14 +467,14 @@ public static Optional solvePNP_SQUARE( if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result"); if (alt != null) - return Optional.of(new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1])); - else return Optional.of(new PNPResults(best, errors[0])); + 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 Optional.empty(); + return new PNPResults(); } finally { // release our Mats from native memory objectPoints.release(); @@ -510,7 +509,7 @@ public static Optional solvePNP_SQUARE( * model points are supplied relative to the origin, this transformation brings the camera to * the origin. */ - public static Optional solvePNP_SQPNP( + public static PNPResults solvePNP_SQPNP( Matrix cameraMatrix, Matrix distCoeffs, List objectTrls, @@ -559,11 +558,11 @@ public static Optional solvePNP_SQPNP( // check if solvePnP failed with NaN results if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result"); - return Optional.of(new PNPResults(best, error[0])); + return new PNPResults(best, error[0]); } catch (Exception e) { System.err.println("SolvePNP_SQPNP failed!"); e.printStackTrace(); - return Optional.empty(); + return new PNPResults(); } } } diff --git a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java index 6a88a4d5f5..0f851c1de4 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java @@ -36,7 +36,11 @@ * 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 +55,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 +75,7 @@ public PNPResults( double ambiguity, double bestReprojErr, double altReprojErr) { + this.isPresent = true; this.best = best; this.alt = alt; this.ambiguity = ambiguity; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java index 3df96f0642..8bc6f93436 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -34,7 +34,6 @@ import java.util.ArrayList; import java.util.List; import java.util.Objects; -import java.util.Optional; import java.util.stream.Collectors; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; @@ -69,10 +68,9 @@ public static List getVisibleLayoutTags( * @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, or an empty Optional - * if estimation fails. + * @return The transformation that maps the field origin to the camera pose. Ensure the {@link PNPResults} are present before utilizing them. */ - public static Optional estimateCamPosePNP( + public static PNPResults estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, List visTags, @@ -81,51 +79,47 @@ public static Optional estimateCamPosePNP( || visTags == null || tagLayout.getTags().size() == 0 || visTags.size() == 0) { - return Optional.empty(); + return new PNPResults(); } var corners = new ArrayList(); for (var tag : visTags) corners.addAll(tag.getDetectedCorners()); var knownTags = getVisibleLayoutTags(visTags, tagLayout); if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { - return Optional.empty(); + return new PNPResults(); } // single-tag pnp if (visTags.size() == 1) { - var camToTag = - OpenCVHelp.solvePNP_SQUARE( - cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners) - .orElse(null); - if (camToTag == null) return Optional.empty(); + 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 o = new Pose3d(); - return Optional.of( - new PNPResults( - new Transform3d(o, bestPose), - new Transform3d(o, altPose), - camToTag.ambiguity, - camToTag.bestReprojErr, - camToTag.altReprojErr)); + return new PNPResults( + new Transform3d(o, bestPose), + new Transform3d(o, altPose), + camToTag.ambiguity, + camToTag.bestReprojErr, + camToTag.altReprojErr + ); } // multi-tag pnp else { var objectTrls = new ArrayList(); for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose)); - var camToOrigin = - OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners).orElse(null); - if (camToOrigin == null) return Optional.empty(); - return Optional.of( - new PNPResults( - camToOrigin.best.inverse(), - camToOrigin.alt.inverse(), - camToOrigin.ambiguity, - camToOrigin.bestReprojErr, - camToOrigin.altReprojErr)); + var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners); + if (!camToOrigin.isPresent) return new PNPResults(); + return new PNPResults( + camToOrigin.best.inverse(), + camToOrigin.alt.inverse(), + camToOrigin.ambiguity, + camToOrigin.bestReprojErr, + camToOrigin.altReprojErr + ); } } } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index ad23dd8127..264f70275a 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -355,9 +355,8 @@ public PhotonPipelineResult process( prop.getIntrinsics(), prop.getDistCoeffs(), tgt.getModel().vertices, - noisyTargetCorners) - .orElse(null); - if (pnpSim == null) continue; + noisyTargetCorners); + if (!pnpSim.isPresent) continue; centerRot = prop.getPixelRot( OpenCVHelp.projectPoints( diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java index ae52dde639..999149d42a 100644 --- a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java +++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java @@ -197,8 +197,7 @@ public void testSolvePNP_SQUARE() { prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, - targetCorners) - .orElseThrow(); + targetCorners); var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); assertSame(actualRelation.camToTarg, estRelation.camToTarg); } @@ -229,8 +228,7 @@ public void testSolvePNP_SQPNP() { prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, - targetCorners) - .orElseThrow(); + targetCorners); var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); assertSame(actualRelation.camToTarg, estRelation.camToTarg); } From 4364ac267df338da0b45287cfef81a8fab44eb46 Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 23 Jul 2023 16:20:38 -0700 Subject: [PATCH 08/13] publish sim intrinsics/distortion to camera --- .../java/org/photonvision/simulation/PhotonCameraSim.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 264f70275a..d68ffd93f7 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -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; @@ -460,6 +460,8 @@ public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimest ts.targetPoseEntry.set(poseData, receiveTimestamp); } + ts.cameraIntrinsicsPublisher.set(prop.getIntrinsics().getData(), receiveTimestamp); + ts.cameraDistortionPublisher.set(prop.getDistCoeffs().getData(), receiveTimestamp); ts.heartbeatPublisher.set(heartbeatCounter++, receiveTimestamp); } } From 1f03b961975324b3ce8aa2fb6a9334b103e08e97 Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 23 Jul 2023 16:24:46 -0700 Subject: [PATCH 09/13] spotless --- .../org/photonvision/PhotonPoseEstimator.java | 2 +- .../photonvision/estimation/PNPResults.java | 12 ++++++-- .../estimation/VisionEstimation.java | 29 ++++++++++--------- .../simulation/PhotonCameraSim.java | 8 ++--- .../java/org/photonvision/OpenCVTest.java | 10 ++----- 5 files changed, 32 insertions(+), 29 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 1e2db3d24a..8bbdaf5744 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -370,7 +370,7 @@ private Optional multiTagPNPStrategy( var pnpResults = VisionEstimation.estimateCamPosePNP( - cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags); + 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); diff --git a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java index 0f851c1de4..89adf94bcf 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java @@ -36,10 +36,18 @@ * 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. */ + /** + * 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. */ + + /** + * 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; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java index 8bc6f93436..dad9eed5b0 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -68,7 +68,8 @@ public static List getVisibleLayoutTags( * @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. Ensure the {@link PNPResults} are present before utilizing them. + * @return The transformation that maps the field origin to the camera pose. Ensure the {@link + * PNPResults} are present before utilizing them. */ public static PNPResults estimateCamPosePNP( Matrix cameraMatrix, @@ -91,7 +92,9 @@ public static PNPResults estimateCamPosePNP( // single-tag pnp if (visTags.size() == 1) { - var camToTag = OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners); + 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(); @@ -100,12 +103,11 @@ public static PNPResults estimateCamPosePNP( var o = new Pose3d(); return new PNPResults( - new Transform3d(o, bestPose), - new Transform3d(o, altPose), - camToTag.ambiguity, - camToTag.bestReprojErr, - camToTag.altReprojErr - ); + new Transform3d(o, bestPose), + new Transform3d(o, altPose), + camToTag.ambiguity, + camToTag.bestReprojErr, + camToTag.altReprojErr); } // multi-tag pnp else { @@ -114,12 +116,11 @@ public static PNPResults estimateCamPosePNP( var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners); if (!camToOrigin.isPresent) return new PNPResults(); return new PNPResults( - camToOrigin.best.inverse(), - camToOrigin.alt.inverse(), - camToOrigin.ambiguity, - camToOrigin.bestReprojErr, - camToOrigin.altReprojErr - ); + camToOrigin.best.inverse(), + camToOrigin.alt.inverse(), + camToOrigin.ambiguity, + camToOrigin.bestReprojErr, + camToOrigin.altReprojErr); } } } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index d68ffd93f7..81a20d0d36 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -352,10 +352,10 @@ public PhotonPipelineResult process( if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP pnpSim = OpenCVHelp.solvePNP_SQUARE( - prop.getIntrinsics(), - prop.getDistCoeffs(), - tgt.getModel().vertices, - noisyTargetCorners); + prop.getIntrinsics(), + prop.getDistCoeffs(), + tgt.getModel().vertices, + noisyTargetCorners); if (!pnpSim.isPresent) continue; centerRot = prop.getPixelRot( diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java index 999149d42a..594439e847 100644 --- a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java +++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java @@ -194,10 +194,7 @@ public void testSolvePNP_SQUARE() { prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); var pnpSim = OpenCVHelp.solvePNP_SQUARE( - prop.getIntrinsics(), - prop.getDistCoeffs(), - target.getModel().vertices, - targetCorners); + prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners); var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); assertSame(actualRelation.camToTarg, estRelation.camToTarg); } @@ -225,10 +222,7 @@ public void testSolvePNP_SQPNP() { prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); var pnpSim = OpenCVHelp.solvePNP_SQPNP( - prop.getIntrinsics(), - prop.getDistCoeffs(), - target.getModel().vertices, - targetCorners); + prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners); var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); assertSame(actualRelation.camToTarg, estRelation.camToTarg); } From 1e393d93eef41ef16020d54076580c0eae0a0c39 Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 23 Jul 2023 18:00:39 -0700 Subject: [PATCH 10/13] multitag pnp estimation unit test --- .../org/photonvision/VisionSystemSimTest.java | 60 +++++++++++++++++++ 1 file changed, 60 insertions(+) diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 77c6859b8b..eb9abafbd7 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -28,6 +28,8 @@ import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.jni.AprilTagJNI; import edu.wpi.first.cscore.CameraServerCvJNI; import edu.wpi.first.cscore.CameraServerJNI; @@ -46,6 +48,8 @@ import edu.wpi.first.util.CombinedRuntimeLoader; import edu.wpi.first.util.RuntimeLoader; import edu.wpi.first.util.WPIUtilJNI; + +import java.util.ArrayList; import java.util.List; import java.util.stream.Stream; import org.junit.jupiter.api.AfterAll; @@ -57,6 +61,8 @@ import org.junit.jupiter.params.provider.MethodSource; import org.junit.jupiter.params.provider.ValueSource; import org.opencv.core.Core; +import org.opencv.highgui.HighGui; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.estimation.TargetModel; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.VisionSystemSim; @@ -482,4 +488,58 @@ public void testMultipleTargets() { tgtList = res.getTargets(); assertEquals(11, tgtList.size()); } + + @Test + public void testPoseEstimation() { + var visionSysSim = new VisionSystemSim("Test"); + var camera = new PhotonCamera("camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90)); + cameraSim.setMinTargetAreaPixels(20.0); + + List tagList = new ArrayList<>(); + tagList.add(new AprilTag(0, new Pose3d(12, 3, 1, new Rotation3d(0, 0, Math.PI)))); + tagList.add(new AprilTag(1, new Pose3d(12, 1, -1, new Rotation3d(0, 0, Math.PI)))); + tagList.add(new AprilTag(2, new Pose3d(11, 0, 2, new Rotation3d(0, 0, Math.PI)))); + double fieldLength = Units.feetToMeters(54.0); + double fieldWidth = Units.feetToMeters(27.0); + AprilTagFieldLayout layout = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth); + PhotonPoseEstimator estimator = + new PhotonPoseEstimator( + layout, + PoseStrategy.MULTI_TAG_PNP, + camera, + new Transform3d()); + Pose2d robotPose = new Pose2d(5, 1, Rotation2d.fromDegrees(5)); + + visionSysSim.addVisionTargets( + new VisionTargetSim( + tagList.get(0).pose, + TargetModel.kTag16h5, + 0)); + + visionSysSim.update(robotPose); + Pose3d pose = estimator.update().get().estimatedPose; + assertEquals(5, pose.getX(), .01); + assertEquals(1, pose.getY(), .01); + assertEquals(0, pose.getZ(), .01); + + visionSysSim.addVisionTargets( + new VisionTargetSim( + tagList.get(1).pose, + TargetModel.kTag16h5, + 1)); + visionSysSim.addVisionTargets( + new VisionTargetSim( + tagList.get(2).pose, + TargetModel.kTag16h5, + 2)); + + visionSysSim.update(robotPose); + pose = estimator.update().get().estimatedPose; + assertEquals(5, pose.getX(), .01); + assertEquals(1, pose.getY(), .01); + assertEquals(0, pose.getZ(), .01); + } } From eb74a45cc67228ff0f58fb8449b81c8aaefd774d Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 23 Jul 2023 18:01:24 -0700 Subject: [PATCH 11/13] spotless --- .../org/photonvision/VisionSystemSimTest.java | 29 +++++-------------- 1 file changed, 7 insertions(+), 22 deletions(-) diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index eb9abafbd7..e2196761ef 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -48,7 +48,6 @@ import edu.wpi.first.util.CombinedRuntimeLoader; import edu.wpi.first.util.RuntimeLoader; import edu.wpi.first.util.WPIUtilJNI; - import java.util.ArrayList; import java.util.List; import java.util.stream.Stream; @@ -61,7 +60,6 @@ import org.junit.jupiter.params.provider.MethodSource; import org.junit.jupiter.params.provider.ValueSource; import org.opencv.core.Core; -import org.opencv.highgui.HighGui; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.estimation.TargetModel; import org.photonvision.simulation.PhotonCameraSim; @@ -506,19 +504,12 @@ public void testPoseEstimation() { double fieldWidth = Units.feetToMeters(27.0); AprilTagFieldLayout layout = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth); PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - layout, - PoseStrategy.MULTI_TAG_PNP, - camera, - new Transform3d()); + new PhotonPoseEstimator(layout, PoseStrategy.MULTI_TAG_PNP, camera, new Transform3d()); Pose2d robotPose = new Pose2d(5, 1, Rotation2d.fromDegrees(5)); - + visionSysSim.addVisionTargets( - new VisionTargetSim( - tagList.get(0).pose, - TargetModel.kTag16h5, - 0)); - + new VisionTargetSim(tagList.get(0).pose, TargetModel.kTag16h5, 0)); + visionSysSim.update(robotPose); Pose3d pose = estimator.update().get().estimatedPose; assertEquals(5, pose.getX(), .01); @@ -526,16 +517,10 @@ public void testPoseEstimation() { assertEquals(0, pose.getZ(), .01); visionSysSim.addVisionTargets( - new VisionTargetSim( - tagList.get(1).pose, - TargetModel.kTag16h5, - 1)); + new VisionTargetSim(tagList.get(1).pose, TargetModel.kTag16h5, 1)); visionSysSim.addVisionTargets( - new VisionTargetSim( - tagList.get(2).pose, - TargetModel.kTag16h5, - 2)); - + new VisionTargetSim(tagList.get(2).pose, TargetModel.kTag16h5, 2)); + visionSysSim.update(robotPose); pose = estimator.update().get().estimatedPose; assertEquals(5, pose.getX(), .01); From 97300db22d016d4cd0d9065efbbb5c1114e5b01a Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 23 Jul 2023 18:12:39 -0700 Subject: [PATCH 12/13] dont use photonposeestimator --- .../java/org/photonvision/VisionSystemSimTest.java | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index e2196761ef..fa0f275067 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -60,8 +60,8 @@ import org.junit.jupiter.params.provider.MethodSource; import org.junit.jupiter.params.provider.ValueSource; import org.opencv.core.Core; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.estimation.TargetModel; +import org.photonvision.estimation.VisionEstimation; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.VisionSystemSim; import org.photonvision.simulation.VisionTargetSim; @@ -503,18 +503,18 @@ public void testPoseEstimation() { double fieldLength = Units.feetToMeters(54.0); double fieldWidth = Units.feetToMeters(27.0); AprilTagFieldLayout layout = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth); - PhotonPoseEstimator estimator = - new PhotonPoseEstimator(layout, PoseStrategy.MULTI_TAG_PNP, camera, new Transform3d()); Pose2d robotPose = new Pose2d(5, 1, Rotation2d.fromDegrees(5)); visionSysSim.addVisionTargets( new VisionTargetSim(tagList.get(0).pose, TargetModel.kTag16h5, 0)); visionSysSim.update(robotPose); - Pose3d pose = estimator.update().get().estimatedPose; + var results = VisionEstimation.estimateCamPosePNP(camera.getCameraMatrix().get(), camera.getDistCoeffs().get(), camera.getLatestResult().getTargets(), layout); + Pose3d pose = new Pose3d().plus(results.best); assertEquals(5, pose.getX(), .01); assertEquals(1, pose.getY(), .01); assertEquals(0, pose.getZ(), .01); + assertEquals(Math.toRadians(5), pose.getRotation().getZ(), 0.01); visionSysSim.addVisionTargets( new VisionTargetSim(tagList.get(1).pose, TargetModel.kTag16h5, 1)); @@ -522,9 +522,11 @@ public void testPoseEstimation() { new VisionTargetSim(tagList.get(2).pose, TargetModel.kTag16h5, 2)); visionSysSim.update(robotPose); - pose = estimator.update().get().estimatedPose; + results = VisionEstimation.estimateCamPosePNP(camera.getCameraMatrix().get(), camera.getDistCoeffs().get(), camera.getLatestResult().getTargets(), layout); + pose = new Pose3d().plus(results.best); assertEquals(5, pose.getX(), .01); assertEquals(1, pose.getY(), .01); assertEquals(0, pose.getZ(), .01); + assertEquals(Math.toRadians(5), pose.getRotation().getZ(), 0.01); } } From b104ae3e32abe40621ad1ed4809f2b055083b7cd Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 23 Jul 2023 18:12:59 -0700 Subject: [PATCH 13/13] spotless --- .../java/org/photonvision/VisionSystemSimTest.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index fa0f275067..6f7720a666 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -509,7 +509,12 @@ public void testPoseEstimation() { new VisionTargetSim(tagList.get(0).pose, TargetModel.kTag16h5, 0)); visionSysSim.update(robotPose); - var results = VisionEstimation.estimateCamPosePNP(camera.getCameraMatrix().get(), camera.getDistCoeffs().get(), camera.getLatestResult().getTargets(), layout); + var results = + VisionEstimation.estimateCamPosePNP( + camera.getCameraMatrix().get(), + camera.getDistCoeffs().get(), + camera.getLatestResult().getTargets(), + layout); Pose3d pose = new Pose3d().plus(results.best); assertEquals(5, pose.getX(), .01); assertEquals(1, pose.getY(), .01); @@ -522,7 +527,12 @@ public void testPoseEstimation() { new VisionTargetSim(tagList.get(2).pose, TargetModel.kTag16h5, 2)); visionSysSim.update(robotPose); - results = VisionEstimation.estimateCamPosePNP(camera.getCameraMatrix().get(), camera.getDistCoeffs().get(), camera.getLatestResult().getTargets(), layout); + results = + VisionEstimation.estimateCamPosePNP( + camera.getCameraMatrix().get(), + camera.getDistCoeffs().get(), + camera.getLatestResult().getTargets(), + layout); pose = new Pose3d().plus(results.best); assertEquals(5, pose.getX(), .01); assertEquals(1, pose.getY(), .01);