diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 3eecf7211..8bbdaf574 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 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> 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); + // 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 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 b834ad445..944bc0708 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -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. * *

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 { *

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

    @@ -405,68 +406,102 @@ public final class OpenCVHelp { 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()); + // 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(); - 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. * - *

    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. @@ -479,46 +514,55 @@ public final class OpenCVHelp { 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); + 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(); + var tvecs = new ArrayList(); + 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(); + } } } 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 6a88a4d5f..89adf94bc 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,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; 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 9cdc1811f..dad9eed5b 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.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 getVisibleLayoutTags( List 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. * *

    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. Ensure the {@link + * PNPResults} are present before utilizing them. */ - @Deprecated public static PNPResults estimateCamPosePNP( Matrix cameraMatrix, Matrix 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(); 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. - * - *

    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; - } - } } 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 710d7f49b..81a20d0d3 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; @@ -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); } } 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 cec341b4d..d142347a9 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 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; } 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 6a1c36cf3..81736d2dd 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 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 diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index f4d5806e4..6f7720a66 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.assertEquals; 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,7 @@ import edu.wpi.first.networktables.NetworkTablesJNI; 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; @@ -58,6 +61,7 @@ import org.junit.jupiter.params.provider.MethodSource; import org.junit.jupiter.params.provider.ValueSource; import org.opencv.core.Core; 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; @@ -144,7 +148,7 @@ class VisionSystemSimTest { 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 +443,7 @@ class VisionSystemSimTest { 5)); visionSysSim.addVisionTargets( new VisionTargetSim( - targetPoseL.transformBy( + targetPoseR.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), TargetModel.kTag16h5, 6)); @@ -482,4 +486,57 @@ class VisionSystemSimTest { 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); + Pose2d robotPose = new Pose2d(5, 1, Rotation2d.fromDegrees(5)); + + visionSysSim.addVisionTargets( + 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); + 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)); + visionSysSim.addVisionTargets( + 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); + 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); + } }