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

2
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -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,

2
photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp
View File

@@ -203,7 +203,7 @@ PhotonPipelineResult PhotonCameraSim::Process(
std::optional<photon::PnpResult> 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);
}

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