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

fix(photon-lib): skip non-fiducial targets in C++ LOWEST_AMBIGUITY (#2494)

Browse Source
This commit is contained in:
JosephTLockwood
2026-05-16 11:26:15 -04:00
committed by GitHub
parent 996ca3649e
commit a6288afcb4
2 changed files with 46 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -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;
}
}

39
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -108,6 +108,45 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
EXPECT_NEAR(2, wpi::units::unit_cast<double>(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<photon::PhotonTrackedTarget> 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<photon::EstimatedRobotPose> 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<double>(pose.X()), .01);
EXPECT_NEAR(3, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, wpi::units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::vector<wpi::apriltag::AprilTag> tags = {
{0, wpi::math::Pose3d(wpi::units::meter_t(3), wpi::units::meter_t(3),