Add Python test harness for openCVHelp class (#1557)

photon-lib/py/test/visionSystemSim_test.py

@@ -20,13 +20,11 @@ from wpimath.units import feetToMeters, meters
 
 @pytest.fixture(autouse=True)
 def setupCommon() -> None:
-
     nt.NetworkTableInstance.getDefault().startServer()
     setVersionCheckEnabled(False)
 
 
 def test_VisibilityCupidShuffle() -> None:
-
     targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi))
 
     visionSysSim = VisionSystemSim("Test")
@@ -84,7 +82,6 @@ def test_VisibilityCupidShuffle() -> None:
 
 
 def test_NotVisibleVert1() -> None:
-
     targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi))
 
     visionSysSim = VisionSystemSim("Test")
@@ -112,7 +109,6 @@ def test_NotVisibleVert1() -> None:
 
 
 def test_NotVisibleVert2() -> None:
-
     targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi))
 
     robotToCamera = Transform3d(
@@ -162,7 +158,6 @@ def test_NotVisibleTargetSize() -> None:
 
 
 def test_NotVisibleTooFarLeds() -> None:
-
     targetPose = Pose3d(Translation3d(15.98, 0.0, 1.0), Rotation3d(0, 0, math.pi))
 
     visionSysSim = VisionSystemSim("Test")
@@ -190,7 +185,6 @@ def test_NotVisibleTooFarLeds() -> None:
     "expected_yaw", [-10.0, -5.0, -2.0, -1.0, 0.0, 5.0, 7.0, 10.23]
 )
 def test_YawAngles(expected_yaw) -> None:
-
     targetPose = Pose3d(
         Translation3d(15.98, 0.0, 1.0), Rotation3d(0.0, 0.0, 3.0 * math.pi / 4.0)
     )
@@ -220,7 +214,6 @@ def test_YawAngles(expected_yaw) -> None:
     "expected_pitch", [-10.0, -5.0, -2.0, -1.0, 0.0, 5.0, 7.0, 10.23]
 )
 def test_PitchAngles(expected_pitch) -> None:
-
     targetPose = Pose3d(
         Translation3d(15.98, 0.0, 0.0), Rotation3d(0, 0, 3.0 * math.pi / 4.0)
     )
@@ -482,3 +475,79 @@ def test_PoseEstimation() -> 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_PoseEstimationRotated() -> None:
+    robotToCamera = Transform3d(
+        Translation3d(6.0 * 0.0254, 6.0 * 0.0254, 6.0 * 0.0254),
+        Rotation3d(0.0, math.radians(-30.0), math.radians(25.5)),
+    )
+
+    visionSysSim = VisionSystemSim("Test")
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, robotToCamera)
+    cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
+    cameraSim.setMinTargetAreaPixels(20.0)
+
+    tagList: list[AprilTag] = []
+    at0 = AprilTag()
+    at0.ID = 0
+    at0.pose = Pose3d(12.0, 3.0, 1.0, Rotation3d(0.0, 0.0, math.pi))
+    tagList.append(at0)
+    at1 = AprilTag()
+    at1.ID = 1
+    at1.pose = Pose3d(12.0, 1.0, -1.0, Rotation3d(0.0, 0.0, math.pi))
+    tagList.append(at1)
+    at2 = AprilTag()
+    at2.ID = 2
+    at2.pose = Pose3d(11.0, 0.0, 2.0, Rotation3d(0.0, 0.0, math.pi))
+    tagList.append(at2)
+
+    fieldLength: meters = 54.0
+    fieldWidth: meters = 27.0
+    layout = AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
+    robotPose = Pose2d(Translation2d(5.0, 1.0), Rotation2d.fromDegrees(-5.0))
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(tagList[0].pose, TargetModel.AprilTag36h11(), 0)]
+    )
+
+    visionSysSim.update(robotPose)
+
+    camEigen = cameraSim.prop.getIntrinsics()
+    distEigen = cameraSim.prop.getDistCoeffs()
+
+    camResults = camera.getLatestResult()
+    targets = camResults.getTargets()
+    results = VisionEstimation.estimateCamPosePNP(
+        camEigen, distEigen, targets, layout, TargetModel.AprilTag36h11()
+    )
+    assert results is not None
+    pose: Pose3d = Pose3d() + results.best
+    pose = pose.transformBy(robotToCamera.inverse())
+    assert pose.X() == pytest.approx(5.0, abs=0.01)
+    assert pose.Y() == pytest.approx(1.0, abs=0.01)
+    assert pose.Z() == pytest.approx(0.0, abs=0.01)
+    assert pose.rotation().Z() == pytest.approx(math.radians(-5.0), abs=0.01)
+
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(tagList[1].pose, TargetModel.AprilTag36h11(), 1)]
+    )
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(tagList[2].pose, TargetModel.AprilTag36h11(), 2)]
+    )
+    visionSysSim.update(robotPose)
+
+    camResults2 = camera.getLatestResult()
+    targets2 = camResults2.getTargets()
+    results2 = VisionEstimation.estimateCamPosePNP(
+        camEigen, distEigen, targets2, layout, TargetModel.AprilTag36h11()
+    )
+    assert results2 is not None
+    pose2 = Pose3d() + results2.best
+    pose2 = pose2.transformBy(robotToCamera.inverse())
+
+    assert pose2.X() == pytest.approx(robotPose.X(), abs=0.01)
+    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)