Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-02 02:51: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

74
photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
View File

@@ -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");
}
}

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);
}