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 fallback for the multitag on rio pose strategy (#1755)

Browse Source
This commit is contained in:
Joseph Eng
2025-02-01 14:09:43 -08:00
committed by GitHub
parent 8ec2da952f
commit 99427d888a
5 changed files with 184 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -419,6 +419,49 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
EXPECT_FALSE(estimatedPose);
}
TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, CopyResult) {
std::vector<photon::PhotonTrackedTarget> targets{};