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

fix(photon-lib): only report actual contributors in targetsUsed (#2493)

Browse Source
This commit is contained in:
JosephTLockwood
2026-05-16 11:26:39 -04:00
committed by GitHub
parent a6288afcb4
commit 6ef0f91600
6 changed files with 296 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
photon-lib/py/photonlibpy/photonPoseEstimator.py
View File

@@ -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:

9
photon-lib/py/test/photonPoseEstimator_test.py
View File

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

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

@@ -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<PhotonTrackedTarget> usedTargets = new ArrayList<>(estimatedRobotPoses.size());
for (Pair<PhotonTrackedTarget, Pose3d> 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));
}

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

@@ -24,6 +24,7 @@
#include "photon/PhotonPoseEstimator.h"
#include <array>
#include <limits>
#include <optional>
#include <utility>
@@ -121,10 +122,11 @@ PhotonPoseEstimator::EstimateLowestAmbiguityPose(
return std::nullopt;
}
std::array<PhotonTrackedTarget, 1> 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<EstimatedRobotPose>
@@ -136,7 +138,8 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
wpi::units::meter_t smallestHeightDifference =
wpi::units::meter_t(std::numeric_limits<double>::infinity());
std::optional<EstimatedRobotPose> pose = std::nullopt;
std::optional<wpi::math::Pose3d> bestPose = std::nullopt;
std::optional<PhotonTrackedTarget> bestTarget = std::nullopt;
for (auto& target : cameraResult.GetTargets()) {
std::optional<wpi::math::Pose3d> 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<PhotonTrackedTarget, 1> usedTargets{*bestTarget};
return EstimatedRobotPose{*bestPose, cameraResult.GetTimestamp(), usedTargets,
CLOSEST_TO_CAMERA_HEIGHT};
}
std::optional<EstimatedRobotPose>
@@ -189,6 +195,7 @@ PhotonPoseEstimator::EstimateClosestToReferencePose(
wpi::units::meter_t(std::numeric_limits<double>::infinity());
wpi::units::second_t stateTimestamp = wpi::units::second_t(0);
wpi::math::Pose3d pose;
std::optional<PhotonTrackedTarget> 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<PhotonTrackedTarget, 1> 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<PhotonTrackedTarget, 1> 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<EstimatedRobotPose>
@@ -411,7 +425,7 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose(
return std::nullopt;
}
std::vector<
std::pair<wpi::math::Pose3d, std::pair<double, wpi::units::second_t>>>
std::pair<wpi::math::Pose3d, std::pair<double, PhotonTrackedTarget>>>
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<PhotonTrackedTarget, 1> 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<wpi::math::Pose3d, std::pair<double, wpi::units::second_t>>&
std::vector<PhotonTrackedTarget> usedTargets;
usedTargets.reserve(tempPoses.size());
for (std::pair<wpi::math::Pose3d, std::pair<double, PhotonTrackedTarget>>&
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<EstimatedRobotPose>

130
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

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

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

@@ -106,6 +106,9 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
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);
// Only the chosen (lowest-ambiguity) target should be reported as used.
EXPECT_EQ(static_cast<size_t>(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<double>(pose.X()), .01);
EXPECT_NEAR(4, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(0, wpi::units::unit_cast<double>(pose.Z()), .01);
// Only the chosen target should be reported as used.
EXPECT_EQ(static_cast<size_t>(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<double>(pose.X()), .01);
EXPECT_NEAR(1.1, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(.9, wpi::units::unit_cast<double>(pose.Z()), .01);
// Only the chosen target should be reported as used.
EXPECT_EQ(static_cast<size_t>(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<double>(pose.X()), .01);
EXPECT_NEAR(1.1, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(1, wpi::units::unit_cast<double>(pose.Z()), .01);
// Only the chosen target should be reported as used.
EXPECT_EQ(static_cast<size_t>(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<double>(pose.Y()), .01);
EXPECT_NEAR(wpi::units::unit_cast<double>(realPose.Z()),
wpi::units::unit_cast<double>(pose.Z()), .01);
// PNP_DISTANCE_TRIG_SOLVE uses only the best target.
EXPECT_EQ(static_cast<size_t>(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<double>(pose.Y()), .01);
EXPECT_NEAR(wpi::units::unit_cast<double>(realPose.Z()),
wpi::units::unit_cast<double>(pose.Z()), .01);
// PNP_DISTANCE_TRIG_SOLVE uses only the best target.
EXPECT_EQ(static_cast<size_t>(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<double>(pose.X()), .01);
EXPECT_NEAR(2.15, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2.15, wpi::units::unit_cast<double>(pose.Z()), .01);
// Only the three fiducial targets contributed; the non-fiducial fourth target
// is excluded.
EXPECT_EQ(static_cast<size_t>(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<photon::PhotonTrackedTarget> 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<photon::EstimatedRobotPose> 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<photon::PhotonTrackedTarget> 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<photon::EstimatedRobotPose> 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<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);
// LOWEST_AMBIGUITY fallback should report only the single chosen target.
EXPECT_EQ(static_cast<size_t>(1), estimatedPose.value().targetsUsed.size());
EXPECT_EQ(1, estimatedPose.value().targetsUsed[0].GetFiducialId());
}
TEST(PhotonPoseEstimatorTest, CopyResult) {