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

[photon-lib] Fix sim tag ambiguity (#1653)

Browse Source
This commit is contained in:
Nolan Brown
2024-12-29 14:43:55 -08:00
committed by GitHub
parent d0e5e169cc
commit b7a2636e97
5 changed files with 105 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
View File

@@ -547,3 +547,30 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
ASSERT_NEAR(units::degree_t{-5}.convert<units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 0.01);
}
TEST_F(VisionSystemSimTest, TestTagAmbiguity) {
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
cameraSim.SetMinTargetAreaPixels(20.0);
frc::Pose3d targetPose{
frc::Translation3d{2_m, 0_m, 0_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
visionSysSim.AddVisionTargets(
{photon::VisionTargetSim{targetPose, photon::kAprilTag36h11, 3}});
frc::Pose2d robotPose{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{0_deg}};
visionSysSim.Update(robotPose);
double ambiguity =
camera.GetLatestResult().GetBestTarget().GetPoseAmbiguity();
ASSERT_TRUE(ambiguity > 0.5);
robotPose =
frc::Pose2d{frc::Translation2d{-2_m, -2_m}, frc::Rotation2d{30_deg}};
visionSysSim.Update(robotPose);
ambiguity = camera.GetLatestResult().GetBestTarget().GetPoseAmbiguity();
ASSERT_TRUE(0 < ambiguity && ambiguity < 0.2);
}