diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py index f487e5523..ef10dbee5 100644 --- a/photon-lib/py/photonlibpy/photonPoseEstimator.py +++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py @@ -187,7 +187,7 @@ class PhotonPoseEstimator: ) return EstimatedRobotPose( - Pose3d(robotPose), result.getTimestampSeconds(), result.getTargets() + Pose3d(robotPose), result.getTimestampSeconds(), [bestTarget] ) def estimateCoprocMultiTagPose( @@ -259,7 +259,7 @@ class PhotonPoseEstimator: lowestAmbiguityTarget.getBestCameraToTarget().inverse() ).transformBy(self.robotToCamera.inverse()), result.getTimestampSeconds(), - result.targets, + [lowestAmbiguityTarget], ) def _reportFiducialPoseError(self, fiducialId: int) -> None: diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py index ad9693664..430258dbe 100644 --- a/photon-lib/py/test/photonPoseEstimator_test.py +++ b/photon-lib/py/test/photonPoseEstimator_test.py @@ -151,6 +151,9 @@ def test_lowestAmbiguityStrategy(): assertEquals(1, pose.x, 0.01) assertEquals(3, pose.y, 0.01) assertEquals(2, pose.z, 0.01) + # Only the chosen (lowest-ambiguity) target should be reported as used. + assert len(estimatedPose.targetsUsed) == 1 + assert estimatedPose.targetsUsed[0].fiducialId == 1 def test_pnpDistanceTrigSolve(): @@ -203,6 +206,9 @@ def test_pnpDistanceTrigSolve(): assertEquals(realPose.x, pose.x, 0.01) assertEquals(realPose.y, pose.y, 0.01) assertEquals(0.0, pose.z, 0.01) + # PNP_DISTANCE_TRIG_SOLVE uses only the best target. + assert len(estimatedRobotPose.targetsUsed) == 1 + assert estimatedRobotPose.targetsUsed[0].fiducialId == bestTarget.fiducialId # Straight on fakeTimestampSecs += 60 @@ -229,6 +235,9 @@ def test_pnpDistanceTrigSolve(): assertEquals(realPose.x, pose.x, 0.01) assertEquals(realPose.y, pose.y, 0.01) assertEquals(0.0, pose.z, 0.01) + # PNP_DISTANCE_TRIG_SOLVE uses only the best target. + assert len(estimatedRobotPose.targetsUsed) == 1 + assert estimatedRobotPose.targetsUsed[0].fiducialId == bestTarget.fiducialId def test_multiTagOnCoprocStrategy(): diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index ea29a83cd..49c221e0e 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -328,7 +328,7 @@ public class PhotonPoseEstimator { new EstimatedRobotPose( new Pose3d(robotPose), cameraResult.getTimestampSeconds(), - cameraResult.getTargets(), + List.of(bestTarget), PoseStrategy.PNP_DISTANCE_TRIG_SOLVE)); } @@ -511,7 +511,7 @@ public class PhotonPoseEstimator { .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), cameraResult.getTimestampSeconds(), - cameraResult.getTargets(), + List.of(lowestAmbiguityTarget), PoseStrategy.LOWEST_AMBIGUITY)); } @@ -529,7 +529,8 @@ public class PhotonPoseEstimator { return Optional.empty(); } double smallestHeightDifference = 10e9; - EstimatedRobotPose closestHeightTarget = null; + Pose3d bestPose = null; + PhotonTrackedTarget bestTarget = null; for (PhotonTrackedTarget target : cameraResult.targets) { int targetFiducialId = target.getFiducialId(); @@ -563,33 +564,33 @@ public class PhotonPoseEstimator { if (alternateTransformDelta < smallestHeightDifference) { smallestHeightDifference = alternateTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - cameraResult.getTimestampSeconds(), - cameraResult.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); + bestPose = + targetPosition + .get() + .transformBy(target.getAlternateCameraToTarget().inverse()) + .transformBy(robotToCamera.inverse()); + bestTarget = target; } if (bestTransformDelta < smallestHeightDifference) { smallestHeightDifference = bestTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - cameraResult.getTimestampSeconds(), - cameraResult.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); + bestPose = + targetPosition + .get() + .transformBy(target.getBestCameraToTarget().inverse()) + .transformBy(robotToCamera.inverse()); + bestTarget = target; } } // Need to null check here in case none of the provided targets are fiducial. - return Optional.ofNullable(closestHeightTarget); + if (bestTarget == null) return Optional.empty(); + return Optional.of( + new EstimatedRobotPose( + bestPose, + cameraResult.getTimestampSeconds(), + List.of(bestTarget), + PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT)); } /** @@ -614,7 +615,8 @@ public class PhotonPoseEstimator { } double smallestPoseDelta = 10e9; - EstimatedRobotPose lowestDeltaPose = null; + Pose3d lowestDeltaPose = null; + PhotonTrackedTarget lowestDeltaTarget = null; for (PhotonTrackedTarget target : cameraResult.targets) { int targetFiducialId = target.getFiducialId(); @@ -647,24 +649,22 @@ public class PhotonPoseEstimator { if (altDifference < smallestPoseDelta) { smallestPoseDelta = altDifference; - lowestDeltaPose = - new EstimatedRobotPose( - altTransformPosition, - cameraResult.getTimestampSeconds(), - cameraResult.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); + lowestDeltaPose = altTransformPosition; + lowestDeltaTarget = target; } if (bestDifference < smallestPoseDelta) { smallestPoseDelta = bestDifference; - lowestDeltaPose = - new EstimatedRobotPose( - bestTransformPosition, - cameraResult.getTimestampSeconds(), - cameraResult.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); + lowestDeltaPose = bestTransformPosition; + lowestDeltaTarget = target; } } - return Optional.ofNullable(lowestDeltaPose); + if (lowestDeltaTarget == null) return Optional.empty(); + return Optional.of( + new EstimatedRobotPose( + lowestDeltaPose, + cameraResult.getTimestampSeconds(), + List.of(lowestDeltaTarget), + PoseStrategy.CLOSEST_TO_REFERENCE_POSE)); } /** @@ -708,7 +708,7 @@ public class PhotonPoseEstimator { .transformBy(target.getBestCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), cameraResult.getTimestampSeconds(), - cameraResult.getTargets(), + List.of(target), PoseStrategy.AVERAGE_BEST_TARGETS)); } @@ -730,6 +730,7 @@ public class PhotonPoseEstimator { if (estimatedRobotPoses.isEmpty()) return Optional.empty(); + List usedTargets = new ArrayList<>(estimatedRobotPoses.size()); for (Pair pair : estimatedRobotPoses) { // Total ambiguity is non-zero confirmed because if it was zero, that pose was // returned. @@ -737,13 +738,14 @@ public class PhotonPoseEstimator { Pose3d estimatedPose = pair.getSecond(); transform = transform.plus(estimatedPose.getTranslation().times(weight)); rotation = rotation.rotateBy(estimatedPose.getRotation().times(weight)); + usedTargets.add(pair.getFirst()); } return Optional.of( new EstimatedRobotPose( new Pose3d(transform, rotation), cameraResult.getTimestampSeconds(), - cameraResult.getTargets(), + usedTargets, PoseStrategy.AVERAGE_BEST_TARGETS)); } diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index a05d721c7..fb10bbe3a 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -24,6 +24,7 @@ #include "photon/PhotonPoseEstimator.h" +#include #include #include #include @@ -121,10 +122,11 @@ PhotonPoseEstimator::EstimateLowestAmbiguityPose( return std::nullopt; } + std::array usedTargets{bestTarget}; return EstimatedRobotPose{ fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), - cameraResult.GetTimestamp(), cameraResult.GetTargets(), LOWEST_AMBIGUITY}; + cameraResult.GetTimestamp(), usedTargets, LOWEST_AMBIGUITY}; } std::optional @@ -136,7 +138,8 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose( wpi::units::meter_t smallestHeightDifference = wpi::units::meter_t(std::numeric_limits::infinity()); - std::optional pose = std::nullopt; + std::optional bestPose = std::nullopt; + std::optional bestTarget = std::nullopt; for (auto& target : cameraResult.GetTargets()) { std::optional fiducialPose = @@ -160,23 +163,26 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose( if (alternativeDifference < smallestHeightDifference) { smallestHeightDifference = alternativeDifference; - pose = EstimatedRobotPose{ + bestPose = targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) - .TransformBy(m_robotToCamera.Inverse()), - cameraResult.GetTimestamp(), cameraResult.GetTargets(), - CLOSEST_TO_CAMERA_HEIGHT}; + .TransformBy(m_robotToCamera.Inverse()); + bestTarget = target; } if (bestDifference < smallestHeightDifference) { smallestHeightDifference = bestDifference; - pose = EstimatedRobotPose{ + bestPose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) - .TransformBy(m_robotToCamera.Inverse()), - cameraResult.GetTimestamp(), cameraResult.GetTargets(), - CLOSEST_TO_CAMERA_HEIGHT}; + .TransformBy(m_robotToCamera.Inverse()); + bestTarget = target; } } - return pose; + if (!bestTarget) { + return std::nullopt; + } + std::array usedTargets{*bestTarget}; + return EstimatedRobotPose{*bestPose, cameraResult.GetTimestamp(), usedTargets, + CLOSEST_TO_CAMERA_HEIGHT}; } std::optional @@ -189,6 +195,7 @@ PhotonPoseEstimator::EstimateClosestToReferencePose( wpi::units::meter_t(std::numeric_limits::infinity()); wpi::units::second_t stateTimestamp = wpi::units::second_t(0); wpi::math::Pose3d pose; + std::optional bestTarget = std::nullopt; auto targets = cameraResult.GetTargets(); for (auto& target : targets) { @@ -217,16 +224,22 @@ PhotonPoseEstimator::EstimateClosestToReferencePose( smallestDifference = alternativeDifference; pose = altPose; stateTimestamp = cameraResult.GetTimestamp(); + bestTarget = target; } if (bestDifference < smallestDifference) { smallestDifference = bestDifference; pose = bestPose; stateTimestamp = cameraResult.GetTimestamp(); + bestTarget = target; } } - return EstimatedRobotPose{pose, stateTimestamp, cameraResult.GetTargets(), + if (!bestTarget) { + return std::nullopt; + } + std::array usedTargets{*bestTarget}; + return EstimatedRobotPose{pose, stateTimestamp, usedTargets, CLOSEST_TO_REFERENCE_POSE}; } @@ -399,9 +412,10 @@ PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose( wpi::math::Pose2d robotPose = wpi::math::Pose2d( fieldToCameraTranslation + camToRobotTranslation, headingSample); + std::array usedTargets{bestTarget}; return EstimatedRobotPose{wpi::math::Pose3d(robotPose), - cameraResult.GetTimestamp(), - cameraResult.GetTargets(), PNP_DISTANCE_TRIG_SOLVE}; + cameraResult.GetTimestamp(), usedTargets, + PNP_DISTANCE_TRIG_SOLVE}; } std::optional @@ -411,7 +425,7 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose( return std::nullopt; } std::vector< - std::pair>> + std::pair>> tempPoses; double totalAmbiguity = 0; @@ -429,33 +443,35 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose( wpi::math::Pose3d targetPose = fiducialPose.value(); // Ambiguity = 0, use that pose if (target.GetPoseAmbiguity() == 0) { + std::array usedTargets{target}; return EstimatedRobotPose{ targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), - cameraResult.GetTimestamp(), cameraResult.GetTargets(), - AVERAGE_BEST_TARGETS}; + cameraResult.GetTimestamp(), usedTargets, AVERAGE_BEST_TARGETS}; } totalAmbiguity += 1. / target.GetPoseAmbiguity(); tempPoses.push_back(std::make_pair( targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()), - std::make_pair(target.GetPoseAmbiguity(), - cameraResult.GetTimestamp()))); + std::make_pair(target.GetPoseAmbiguity(), target))); } wpi::math::Translation3d transform = wpi::math::Translation3d(); wpi::math::Rotation3d rotation = wpi::math::Rotation3d(); - for (std::pair>& + std::vector usedTargets; + usedTargets.reserve(tempPoses.size()); + for (std::pair>& pair : tempPoses) { double const weight = (1. / pair.second.first) / totalAmbiguity; transform = transform + pair.first.Translation() * weight; rotation = rotation.RotateBy(pair.first.Rotation() * weight); + usedTargets.push_back(pair.second.second); } return EstimatedRobotPose{wpi::math::Pose3d(transform, rotation), - cameraResult.GetTimestamp(), - cameraResult.GetTargets(), AVERAGE_BEST_TARGETS}; + cameraResult.GetTimestamp(), usedTargets, + AVERAGE_BEST_TARGETS}; } std::optional diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 0411302d7..b698fc697 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -25,6 +25,7 @@ package org.photonvision; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertNotNull; import static org.junit.jupiter.api.Assertions.assertTrue; import static org.junit.jupiter.api.Assertions.fail; @@ -173,6 +174,9 @@ class PhotonPoseEstimatorTest { assertEquals(1, pose.getX(), .01); assertEquals(3, pose.getY(), .01); assertEquals(2, pose.getZ(), .01); + // Only the chosen (lowest-ambiguity) target should be reported as used. + assertEquals(1, estimatedPose.get().targetsUsed.size()); + assertEquals(1, estimatedPose.get().targetsUsed.get(0).getFiducialId()); } @Test @@ -260,6 +264,9 @@ class PhotonPoseEstimatorTest { assertEquals(4, pose.getX(), .01); assertEquals(4, pose.getY(), .01); assertEquals(0, pose.getZ(), .01); + // Only the chosen target should be reported as used. + assertEquals(1, estimatedPose.get().targetsUsed.size()); + assertEquals(1, estimatedPose.get().targetsUsed.get(0).getFiducialId()); } @Test @@ -348,6 +355,9 @@ class PhotonPoseEstimatorTest { assertEquals(1, pose.getX(), .01); assertEquals(1.1, pose.getY(), .01); assertEquals(.9, pose.getZ(), .01); + // Only the chosen target should be reported as used. + assertEquals(1, estimatedPose.get().targetsUsed.size()); + assertEquals(0, estimatedPose.get().targetsUsed.get(0).getFiducialId()); } @Test @@ -510,6 +520,9 @@ class PhotonPoseEstimatorTest { assertEquals(.9, pose.getX(), .01); assertEquals(1.1, pose.getY(), .01); assertEquals(1, pose.getZ(), .01); + // Only the chosen target should be reported as used. + assertEquals(1, estimatedPose.get().targetsUsed.size()); + assertEquals(0, estimatedPose.get().targetsUsed.get(0).getFiducialId()); } @Test @@ -549,6 +562,10 @@ class PhotonPoseEstimatorTest { assertEquals(realPose.getX(), pose.getX(), .01); assertEquals(realPose.getY(), pose.getY(), .01); assertEquals(0.0, pose.getZ(), .01); + // PNP_DISTANCE_TRIG_SOLVE uses only the best target. + assertEquals(1, estimatedPose.get().targetsUsed.size()); + assertEquals( + bestTarget.getFiducialId(), estimatedPose.get().targetsUsed.get(0).getFiducialId()); /* Straight on */ Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero); @@ -642,7 +659,30 @@ class PhotonPoseEstimatorTest { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); // 3 3 3 ambig .4 + new TargetCorner(7, 8))), // 3 3 3 ambig .4 + // Non-fiducial target: must be skipped by the contributor loop and + // therefore must not appear in targetsUsed. + new PhotonTrackedTarget( + 0.0, + 0.0, + 0.0, + 0.0, + -1, + -1, + -1, + new Transform3d(), + new Transform3d(), + 0.5, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -656,6 +696,91 @@ class PhotonPoseEstimatorTest { assertEquals(2.15, pose.getX(), .01); assertEquals(2.15, pose.getY(), .01); assertEquals(2.15, pose.getZ(), .01); + // Only the three fiducial targets contributed; the non-fiducial fourth target is excluded. + assertEquals(3, estimatedPose.get().targetsUsed.size()); + for (PhotonTrackedTarget t : estimatedPose.get().targetsUsed) { + assertTrue(t.getFiducialId() != -1); + } + } + + @Test + void closestToCameraHeightReturnsEmptyForNoFiducialTargets() { + // A single non-fiducial target (fid = -1) should yield no estimate. + cameraOne.result = + new PhotonPipelineResult( + 0, + 4_000_000, + 1_100_000, + 1024, + List.of( + new PhotonTrackedTarget( + 0.0, + 0.0, + 0.0, + 0.0, + -1, + -1, + -1, + new Transform3d(), + new Transform3d(), + 0.5, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + + PhotonPoseEstimator estimator = + new PhotonPoseEstimator( + aprilTags, new Transform3d(new Translation3d(0, 0, 4), new Rotation3d())); + + assertFalse(estimator.estimateClosestToCameraHeightPose(cameraOne.result).isPresent()); + } + + @Test + void closestToReferencePoseReturnsEmptyForNoFiducialTargets() { + cameraOne.result = + new PhotonPipelineResult( + 0, + 17_000_000, + 1_100_000, + 1024, + List.of( + new PhotonTrackedTarget( + 0.0, + 0.0, + 0.0, + 0.0, + -1, + -1, + -1, + new Transform3d(), + new Transform3d(), + 0.5, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + + PhotonPoseEstimator estimator = + new PhotonPoseEstimator( + aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); + + assertFalse( + estimator + .estimateClosestToReferencePose(cameraOne.result, new Pose3d(1, 1, 1, new Rotation3d())) + .isPresent()); } @Test @@ -725,6 +850,9 @@ class PhotonPoseEstimatorTest { assertEquals(1, pose.getX(), 1e-9); assertEquals(3, pose.getY(), 1e-9); assertEquals(2, pose.getZ(), 1e-9); + // LOWEST_AMBIGUITY fallback should report only the single chosen target. + assertEquals(1, estimatedPose.get().targetsUsed.size()); + assertEquals(1, estimatedPose.get().targetsUsed.get(0).getFiducialId()); } @Test diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 64bdb46ae..1cf1ae714 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -106,6 +106,9 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { 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); + // Only the chosen (lowest-ambiguity) target should be reported as used. + EXPECT_EQ(static_cast(1), estimatedPose.value().targetsUsed.size()); + EXPECT_EQ(1, estimatedPose.value().targetsUsed[0].GetFiducialId()); } TEST(PhotonPoseEstimatorTest, LowestAmbiguityIgnoresNonFiducialTargets) { @@ -206,6 +209,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { EXPECT_NEAR(4, wpi::units::unit_cast(pose.X()), .01); EXPECT_NEAR(4, wpi::units::unit_cast(pose.Y()), .01); EXPECT_NEAR(0, wpi::units::unit_cast(pose.Z()), .01); + // Only the chosen target should be reported as used. + EXPECT_EQ(static_cast(1), estimatedPose.value().targetsUsed.size()); + EXPECT_EQ(1, estimatedPose.value().targetsUsed[0].GetFiducialId()); } TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { @@ -256,6 +262,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { EXPECT_NEAR(1, wpi::units::unit_cast(pose.X()), .01); EXPECT_NEAR(1.1, wpi::units::unit_cast(pose.Y()), .01); EXPECT_NEAR(.9, wpi::units::unit_cast(pose.Z()), .01); + // Only the chosen target should be reported as used. + EXPECT_EQ(static_cast(1), estimatedPose.value().targetsUsed.size()); + EXPECT_EQ(0, estimatedPose.value().targetsUsed[0].GetFiducialId()); } TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { @@ -342,6 +351,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { EXPECT_NEAR(.9, wpi::units::unit_cast(pose.X()), .01); EXPECT_NEAR(1.1, wpi::units::unit_cast(pose.Y()), .01); EXPECT_NEAR(1, wpi::units::unit_cast(pose.Z()), .01); + // Only the chosen target should be reported as used. + EXPECT_EQ(static_cast(1), estimatedPose.value().targetsUsed.size()); + EXPECT_EQ(0, estimatedPose.value().targetsUsed[0].GetFiducialId()); } TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { @@ -389,6 +401,8 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { wpi::units::unit_cast(pose.Y()), .01); EXPECT_NEAR(wpi::units::unit_cast(realPose.Z()), wpi::units::unit_cast(pose.Z()), .01); + // PNP_DISTANCE_TRIG_SOLVE uses only the best target. + EXPECT_EQ(static_cast(1), estimatedPose.value().targetsUsed.size()); /* Straight on */ wpi::math::Transform3d straightOnTestTransform = wpi::math::Transform3d( @@ -419,6 +433,8 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { wpi::units::unit_cast(pose.Y()), .01); EXPECT_NEAR(wpi::units::unit_cast(realPose.Z()), wpi::units::unit_cast(pose.Z()), .01); + // PNP_DISTANCE_TRIG_SOLVE uses only the best target. + EXPECT_EQ(static_cast(1), estimatedPose.value().targetsUsed.size()); } TEST(PhotonPoseEstimatorTest, AverageBestPoses) { @@ -445,7 +461,13 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; + 0.4, corners, detectedCorners}, + // Non-fiducial target: must be skipped by the contributor loop and + // therefore must not appear in targetsUsed. + photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, -1, -1, -1.f, + wpi::math::Transform3d(), + wpi::math::Transform3d(), 0.5, corners, + detectedCorners}}; cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ @@ -468,6 +490,58 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { EXPECT_NEAR(2.15, wpi::units::unit_cast(pose.X()), .01); EXPECT_NEAR(2.15, wpi::units::unit_cast(pose.Y()), .01); EXPECT_NEAR(2.15, wpi::units::unit_cast(pose.Z()), .01); + // Only the three fiducial targets contributed; the non-fiducial fourth target + // is excluded. + EXPECT_EQ(static_cast(3), estimatedPose.value().targetsUsed.size()); + for (const auto& t : estimatedPose.value().targetsUsed) { + EXPECT_NE(-1, t.GetFiducialId()); + } +} + +TEST(PhotonPoseEstimatorTest, + ClosestToCameraHeightReturnsEmptyForNoFiducialTargets) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + + // A single non-fiducial target (fid = -1) should yield no estimate. + std::vector targets{photon::PhotonTrackedTarget{ + 0.0, 0.0, 0.0, 0.0, -1, -1, -1.f, wpi::math::Transform3d(), + wpi::math::Transform3d(), 0.5, 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(4)); + + photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}}); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.EstimateClosestToCameraHeightPose(result); + } + EXPECT_FALSE(estimatedPose); +} + +TEST(PhotonPoseEstimatorTest, + ClosestToReferencePoseReturnsEmptyForNoFiducialTargets) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + + std::vector targets{photon::PhotonTrackedTarget{ + 0.0, 0.0, 0.0, 0.0, -1, -1, -1.f, wpi::math::Transform3d(), + wpi::math::Transform3d(), 0.5, 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(17)); + + photon::PhotonPoseEstimator estimator(aprilTags, {}); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.EstimateClosestToReferencePose( + result, wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d())); + } + EXPECT_FALSE(estimatedPose); } TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { @@ -513,6 +587,9 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { 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); + // LOWEST_AMBIGUITY fallback should report only the single chosen target. + EXPECT_EQ(static_cast(1), estimatedPose.value().targetsUsed.size()); + EXPECT_EQ(1, estimatedPose.value().targetsUsed[0].GetFiducialId()); } TEST(PhotonPoseEstimatorTest, CopyResult) {