diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index e51d0fe14..91cc8173f 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -379,22 +379,28 @@ public class PhotonPoseEstimator { } for (var target : result.getTargets()) { - visCorners.addAll(target.getDetectedCorners()); - + // Check if tag actually exists in the field layout var tagPoseOpt = fieldTags.getTagPose(target.getFiducialId()); if (tagPoseOpt.isEmpty()) { reportFiducialPoseError(target.getFiducialId()); continue; } - var tagPose = tagPoseOpt.get(); + // 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 + return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); + } + var pnpResults = - VisionEstimation.estimateCamPosePNP( + VisionEstimation.estimateCamPoseSqpnp( cameraMatrixOpt.get(), distCoeffsOpt.get(), visCorners, knownVisTags); var best = new Pose3d() 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 82051d8bc..bc3a735e9 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -45,6 +45,7 @@ public class VisionEstimation { * 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??) * * @param cameraMatrix the camera intrinsics matrix in standard opencv form * @param distCoeffs the camera distortion matrix in standard opencv form @@ -52,6 +53,7 @@ public class VisionEstimation { * @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 */ + @Deprecated public static PNPResults estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, @@ -109,43 +111,40 @@ public class VisionEstimation { } } - // /** - // * The best estimated transformation to the target, and possibly an alternate - // * transformation - // * depending on the solvePNP method. If an alternate solution is present, the - // * ambiguity value - // * represents the ratio of reprojection error in the best solution to the - // * alternate (best / alternate). - // */ - // public static class PNPResults { - // public final Transform3d best; - // public final double bestReprojErr; - - // /** - // * Alternate, ambiguous solution from solvepnp. This may be empty - // * if no alternate solution is found. - // */ - // public final Transform3d alt; - // /** If no alternate solution is found, this is 0 */ - // public final double altReprojErr; - - // /** If no alternate solution is found, this is 0 */ - // public final double ambiguity; - - // public PNPResults() { - // this(new Transform3d(), new Transform3d(), 0, 0, 0); - // } - - // public PNPResults( - // Transform3d best, Transform3d alt, - // double ambiguity, double bestReprojErr, double altReprojErr) { - // this.best = best; - // this.alt = alt; - // this.ambiguity = ambiguity; - // this.bestReprojErr = bestReprojErr; - // this.altReprojErr = 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(kTagModel.getFieldVertices(tag.pose)); + 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 diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index b1d242e44..c2dc8bac5 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -391,8 +391,9 @@ std::optional PhotonPoseEstimator::MultiTagPnpStrategy( } } - if (imagePoints.empty()) { - return std::nullopt; + // We should only do multi-tag if at least 2 tags (* 4 corners/tag) + if (imagePoints.size() < 8) { + return Update(result, camMat, distCoeffs, this->multiTagFallbackStrategy); } // Use OpenCV ITERATIVE solver