diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 53cde3088..a05d721c7 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -95,9 +95,14 @@ PhotonPoseEstimator::EstimateLowestAmbiguityPose( auto targets = cameraResult.GetTargets(); auto foundIt = targets.end(); for (auto it = targets.begin(); it != targets.end(); ++it) { - if (it->GetPoseAmbiguity() < lowestAmbiguityScore) { + double targetPoseAmbiguity = it->GetPoseAmbiguity(); + // Skip non-fiducial targets (ambiguity == -1), otherwise they win over + // every fiducial target and the whole estimate is thrown away when + // GetTagPose(-1) returns nullopt. + if (targetPoseAmbiguity != -1 && + targetPoseAmbiguity < lowestAmbiguityScore) { foundIt = it; - lowestAmbiguityScore = it->GetPoseAmbiguity(); + lowestAmbiguityScore = targetPoseAmbiguity; } } diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 342d9aa38..64bdb46ae 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -108,6 +108,45 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { EXPECT_NEAR(2, wpi::units::unit_cast(pose.Z()), .01); } +TEST(PhotonPoseEstimatorTest, LowestAmbiguityIgnoresNonFiducialTargets) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + + // A non-fiducial target reports poseAmbiguity = -1. Without the guard the + // -1 score beats every fiducial target's score, the loop selects the + // non-fiducial target, GetTagPose(-1) returns nullopt, and the whole + // estimate is thrown away. The valid fiducial target (fid=1) must win. + std::vector targets{ + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.3, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 0.0, 0.0, 0.0, 0.0, -1, -1, -1.f, wpi::math::Transform3d(), + wpi::math::Transform3d(), -1.0, corners, detectedCorners}}; + + cameraOne.test = true; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11)); + + photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{}); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.EstimateLowestAmbiguityPose(result); + } + ASSERT_TRUE(estimatedPose); + // Tag 1 is at (5,5,5), bestCameraToTarget = (4,2,3), so the estimated + // robot pose lands at (1,3,2) in the field frame. + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; + EXPECT_NEAR(1, wpi::units::unit_cast(pose.X()), .01); + EXPECT_NEAR(3, wpi::units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(2, wpi::units::unit_cast(pose.Z()), .01); +} + TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { std::vector tags = { {0, wpi::math::Pose3d(wpi::units::meter_t(3), wpi::units::meter_t(3),