diff --git a/photon-lib/py/test/visionSystemSim_test.py b/photon-lib/py/test/visionSystemSim_test.py index 40e68fb49..0c8b3cc05 100644 --- a/photon-lib/py/test/visionSystemSim_test.py +++ b/photon-lib/py/test/visionSystemSim_test.py @@ -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" diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index e71d489f1..d71eecaeb 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -424,7 +424,7 @@ public class PhotonCameraSim implements AutoCloseable { var pnpSim = new PnpResult(); if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP pnpSim = - OpenCVHelp.solvePNP_SQPNP( + OpenCVHelp.solvePNP_SQUARE( prop.getIntrinsics(), prop.getDistCoeffs(), tgt.getModel().vertices, diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp index 46c13ffad..165fc200a 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -203,7 +203,7 @@ PhotonPipelineResult PhotonCameraSim::Process( std::optional pnpSim = std::nullopt; if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) { - pnpSim = OpenCVHelp::SolvePNP_SQPNP( + pnpSim = OpenCVHelp::SolvePNP_Square( prop.GetIntrinsics(), prop.GetDistCoeffs(), tgt.GetModel().GetVertices(), noisyTargetCorners); } diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 04a3ae4cc..fd7f79814 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -136,52 +136,59 @@ class VisionSystemSimTest { // To the right, to the right var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70)); visionSysSim.update(robotPose); + var result = waitForSequenceNumber(camera, 1); - assertFalse(camera.getLatestResult().hasTargets()); + assertFalse(result.hasTargets()); // To the right, to the right robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-95)); visionSysSim.update(robotPose); + result = waitForSequenceNumber(camera, 2); - assertFalse(camera.getLatestResult().hasTargets()); + assertFalse(result.hasTargets()); // To the left, to the left robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(90)); visionSysSim.update(robotPose); + result = waitForSequenceNumber(camera, 3); - assertFalse(camera.getLatestResult().hasTargets()); + assertFalse(result.hasTargets()); // To the left, to the left robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(65)); visionSysSim.update(robotPose); + result = waitForSequenceNumber(camera, 4); - assertFalse(camera.getLatestResult().hasTargets()); + assertFalse(result.hasTargets()); // now kick, now kick robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); + result = waitForSequenceNumber(camera, 5); - waitForSequenceNumber(camera, 5); - assertTrue(camera.getLatestResult().hasTargets()); + assertTrue(result.hasTargets()); // now kick, now kick robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-5)); visionSysSim.update(robotPose); + result = waitForSequenceNumber(camera, 6); - assertTrue(camera.getLatestResult().hasTargets()); + assertTrue(result.hasTargets()); // now walk it by yourself robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-179)); visionSysSim.update(robotPose); + result = waitForSequenceNumber(camera, 7); - assertFalse(camera.getLatestResult().hasTargets()); + assertFalse(result.hasTargets()); // now walk it by yourself visionSysSim.adjustCamera( cameraSim, new Transform3d(new Translation3d(), new Rotation3d(0, 0, Math.PI))); visionSysSim.update(robotPose); + result = waitForSequenceNumber(camera, 8); - assertTrue(camera.getLatestResult().hasTargets()); + assertTrue(result.hasTargets()); } @Test @@ -369,7 +376,7 @@ class VisionSystemSimTest { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI * 0.98)); final var robotPose = - new Pose3d(new Translation3d(15.98 - Units.feetToMeters(testDist), 0, 0), new Rotation3d()); + new Pose3d(new Translation3d(15.98 - Units.feetToMeters(testDist), 0, 0), Rotation3d.kZero); final var robotToCamera = new Transform3d( new Translation3d(0, 0, Units.feetToMeters(testHeight)), @@ -431,67 +438,67 @@ class VisionSystemSimTest { visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( - new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 0.00), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 1)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseC.transformBy( - new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 0.00), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 2)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseR.transformBy( - new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 0.00), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 3)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( - new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 1.00), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 4)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseC.transformBy( - new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 1.00), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 5)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseR.transformBy( - new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 1.00), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 6)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( - new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 0.50), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 7)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseC.transformBy( - new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 0.50), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 8)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( - new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 0.75), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 9)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseR.transformBy( - new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 0.75), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 10)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( - new Transform3d(new Translation3d(0, 0, 0.25), new Rotation3d())), + new Transform3d(new Translation3d(0, 0, 0.25), Rotation3d.kZero)), TargetModel.kAprilTag16h5, 11)); @@ -554,7 +561,7 @@ class VisionSystemSimTest { VisionEstimation.estimateCamPosePNP( camera.getCameraMatrix().get(), camera.getDistCoeffs().get(), - camera.getLatestResult().getTargets(), + waitForSequenceNumber(camera, 2).getTargets(), layout, TargetModel.kAprilTag16h5) .get(); @@ -564,4 +571,27 @@ class VisionSystemSimTest { assertEquals(0, pose.getZ(), .01); assertEquals(Math.toRadians(5), pose.getRotation().getZ(), 0.01); } + + @Test + public void testTagAmbiguity() { + var visionSysSim = new VisionSystemSim("Test"); + var camera = new PhotonCamera(inst, "camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); + cameraSim.setMinTargetAreaPixels(20.0); + + final var targetPose = new Pose3d(new Translation3d(2, 0, 0), new Rotation3d(0, 0, Math.PI)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, TargetModel.kAprilTag36h11, 3)); + + var robotPose = Pose2d.kZero; + visionSysSim.update(robotPose); + double ambiguity = waitForSequenceNumber(camera, 1).getBestTarget().getPoseAmbiguity(); + assertTrue(ambiguity > 0.5, "Tag ambiguity expected to be high"); + + robotPose = new Pose2d(-2, -2, Rotation2d.fromDegrees(30)); + visionSysSim.update(robotPose); + ambiguity = waitForSequenceNumber(camera, 2).getBestTarget().getPoseAmbiguity(); + assertTrue(0 < ambiguity && ambiguity < 0.2, "Tag ambiguity expected to be low"); + } } diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp index 06bf25e6d..88696414a 100644 --- a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -547,3 +547,30 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { ASSERT_NEAR(units::degree_t{-5}.convert().to(), pose2.Rotation().Z().to(), 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); +}