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