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

Don't add bad corners to multi-tag solution (#817)

Browse Source 
Closes #837


---------

Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>
This commit is contained in:
Matt
2023-06-17 22:16:10 -07:00
committed by GitHub
parent b1546b8038
commit f6756bdb9a
3 changed files with 49 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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()