Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-27 02:01:40 +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

46
photon-lib/py/test/visionSystemSim_test.py
View File

@@ -25,8 +25,6 @@ def test_VisibilityCupidShuffle() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
visionSysSim.addVisionTargets(
@@ -88,8 +86,6 @@ def test_NotVisibleVert1() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
visionSysSim.addVisionTargets(
@@ -125,8 +121,6 @@ def test_NotVisibleVert2() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, robotToCamera)
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(
4774, 4774, fovDiag=Rotation2d.fromDegrees(80.0)
)
@@ -155,8 +149,6 @@ def test_NotVisibleTargetSize() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.setMinTargetAreaPixels(20.0)
visionSysSim.addVisionTargets(
@@ -184,8 +176,6 @@ def test_NotVisibleTooFarLeds() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.setMinTargetAreaPixels(1.0)
cameraSim.setMaxSightRange(10.0)
@@ -220,8 +210,6 @@ def test_YawAngles(expected_yaw) -> None:
visionSysSim.addCamera(cameraSim, Transform3d())
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.setMinTargetAreaPixels(0.0)
visionSysSim.addVisionTargets(
@@ -257,8 +245,6 @@ def test_PitchAngles(expected_pitch) -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(
640, 480, fovDiag=Rotation2d.fromDegrees(120.0)
)
@@ -327,8 +313,6 @@ def test_distanceCalc(distParam, pitchParam, heightParam) -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(
640, 480, fovDiag=Rotation2d.fromDegrees(160.0)
)
@@ -366,8 +350,6 @@ def test_MultipleTargets() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.setMinTargetAreaPixels(20.0)
@@ -466,8 +448,6 @@ def test_PoseEstimation() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
cameraSim.setMinTargetAreaPixels(20.0)
@@ -543,8 +523,6 @@ def test_PoseEstimationRotated() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, robotToCamera)
# Set massive FPS so timing isn't an issue
cameraSim.prop.setFPS(1e6)
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
cameraSim.setMinTargetAreaPixels(20.0)
@@ -609,3 +587,27 @@ def test_PoseEstimationRotated() -> None:
assert pose2.Y() == pytest.approx(robotPose.Y(), abs=0.01)
assert pose2.Z() == pytest.approx(0.0, abs=0.01)
assert pose2.rotation().Z() == pytest.approx(math.radians(-5.0), abs=0.01)
def test_TagAmbiguity() -> None:
visionSysSim = VisionSystemSim("Test")
camera = PhotonCamera("camera")
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.setMinTargetAreaPixels(20.0)
targetPose = Pose3d(Translation3d(2.0, 0.0, 0.0), Rotation3d(0, 0, math.pi))
visionSysSim.addVisionTargets(
[VisionTargetSim(targetPose, TargetModel.AprilTag36h11(), 3)]
)
robotPose = Pose2d()
visionSysSim.update(robotPose)
ambiguity = camera.getLatestResult().getBestTarget().getPoseAmbiguity()
assert ambiguity > 0.5, "Tag ambiguity expected to be high"
robotPose = Pose2d(Translation2d(-2.0, -2.0), Rotation2d.fromDegrees(30.0))
visionSysSim.update(robotPose)
ambiguity = camera.getLatestResult().getBestTarget().getPoseAmbiguity()
assert 0 < ambiguity < 0.2, "Tag ambiguity expected to be high"