diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 119aa3155..e51d0fe14 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -367,11 +367,14 @@ public class PhotonPoseEstimator { // Arrays we need declared up front var visCorners = new ArrayList(); var knownVisTags = new ArrayList(); - var fieldToCams = new ArrayList(); - var fieldToCamsAlt = new ArrayList(); if (result.getTargets().size() < 2) { - // Run fallback strategy instead + return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); + } + + boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); + + if (!hasCalibData) { return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); } @@ -388,33 +391,18 @@ public class PhotonPoseEstimator { // actual layout poses of visible tags -- not exposed, so have to recreate knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose)); - - fieldToCams.add(tagPose.transformBy(target.getBestCameraToTarget().inverse())); - fieldToCamsAlt.add(tagPose.transformBy(target.getAlternateCameraToTarget().inverse())); } - boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); + var pnpResults = + VisionEstimation.estimateCamPosePNP( + cameraMatrixOpt.get(), distCoeffsOpt.get(), visCorners, knownVisTags); + var best = + new Pose3d() + .plus(pnpResults.best) // field-to-camera + .plus(robotToCamera.inverse()); // field-to-robot - // multi-target solvePNP - if (hasCalibData) { - var cameraMatrix = cameraMatrixOpt.get(); - var distCoeffs = distCoeffsOpt.get(); - var pnpResults = - VisionEstimation.estimateCamPosePNP(cameraMatrix, distCoeffs, visCorners, knownVisTags); - var best = - new Pose3d() - .plus(pnpResults.best) // field-to-camera - .plus(robotToCamera.inverse()); // field-to-robot - // var alt = new Pose3d() - // .plus(pnpResults.alt) // field-to-camera - // .plus(robotToCamera.inverse()); // field-to-robot - - return Optional.of( - new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets())); - } else { - // TODO fallback strategy? Should we just always do solvePNP? - return Optional.empty(); - } + return Optional.of( + new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets())); } /** diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index 3a06ca9a7..b1d242e44 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -359,11 +359,17 @@ std::optional PhotonPoseEstimator::MultiTagPnpStrategy( std::optional distCoeffs) { using namespace frc; + // Need at least 2 targets if (!result.HasTargets() || result.GetTargets().size() < 2) { return Update(result, std::nullopt, std::nullopt, this->multiTagFallbackStrategy); } + if (!camMat || !distCoeffs) { + return Update(result, std::nullopt, std::nullopt, + this->multiTagFallbackStrategy); + } + auto const targets = result.GetTargets(); // List of corners mapped from 3d space (meters) to the 2d camera screen @@ -393,10 +399,6 @@ std::optional PhotonPoseEstimator::MultiTagPnpStrategy( cv::Mat const rvec(3, 1, cv::DataType::type); cv::Mat const tvec(3, 1, cv::DataType::type); - if (!camMat || !distCoeffs) { - return std::nullopt; - } - cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(), rvec, tvec, false, cv::SOLVEPNP_SQPNP);