Add Python test harness for openCVHelp class (#1557)

photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py

@@ -24,9 +24,32 @@ class RotTrlTransform3d:
     def getRotation(self) -> Rotation3d:
         return self.rot
 
-    def apply(self, trlToApply: Translation3d) -> Translation3d:
+    def applyTranslation(self, trlToApply: Translation3d) -> Translation3d:
         return trlToApply.rotateBy(self.rot) + self.trl
 
+    def applyRotation(self, rotToApply: Rotation3d) -> Rotation3d:
+        return rotToApply + self.rot
+
+    def applyPose(self, poseToApply: Pose3d) -> Pose3d:
+        return Pose3d(
+            self.applyTranslation(poseToApply.translation()),
+            self.applyRotation(poseToApply.rotation()),
+        )
+
+    def applyTrls(self, rots: list[Rotation3d]) -> list[Rotation3d]:
+        retVal: list[Rotation3d] = []
+        for rot in rots:
+            retVal.append(self.applyRotation(rot))
+        return retVal
+
     @classmethod
     def makeRelativeTo(cls, pose: Pose3d) -> Self:
         return cls(pose.rotation(), pose.translation()).inverse()
+
+    @classmethod
+    def makeBetweenPoses(cls, initial: Pose3d, last: Pose3d) -> Self:
+        return cls(
+            last.rotation() - initial.rotation(),
+            last.translation()
+            - initial.translation().rotateBy(last.rotation() - initial.rotation()),
+        )

photon-lib/py/photonlibpy/estimation/targetModel.py

@@ -107,7 +107,7 @@ class TargetModel:
         retVal = []
 
         for vert in self.vertices:
-            retVal.append(basisChange.apply(vert))
+            retVal.append(basisChange.applyTranslation(vert))
 
         return retVal
 

photon-lib/py/photonlibpy/simulation/simCameraProperties.py

@@ -226,8 +226,8 @@ class SimCameraProperties:
     def getVisibleLine(
         self, camRt: RotTrlTransform3d, a: Translation3d, b: Translation3d
     ) -> typing.Tuple[float | None, float | None]:
-        relA = camRt.apply(a)
-        relB = camRt.apply(b)
+        relA = camRt.applyTranslation(a)
+        relB = camRt.applyTranslation(b)
 
         if relA.X() <= 0.0 and relB.X() <= 0.0:
             return (None, None)
@@ -279,7 +279,7 @@ class SimCameraProperties:
                     ipts[i] = None
                     break
 
-            if not ipts[i]:
+            if ipts[i] is None:
                 continue
 
             for j in range(i - 1, 0 - 1):

photon-lib/py/test/openCVHelp_test.py

@@ -0,0 +1,205 @@
+import math
+
+import ntcore as nt
+import pytest
+from photonlibpy.estimation import RotTrlTransform3d, TargetModel
+from photonlibpy.estimation.openCVHelp import OpenCVHelp
+from photonlibpy.photonCamera import setVersionCheckEnabled
+from photonlibpy.simulation import SimCameraProperties, VisionTargetSim
+from wpimath.geometry import Pose3d, Rotation3d, Translation3d
+
+
+@pytest.fixture(autouse=True)
+def setupCommon() -> None:
+    nt.NetworkTableInstance.getDefault().startServer()
+    setVersionCheckEnabled(False)
+
+
+def test_TrlConvert():
+    trl = Translation3d(0.75, -0.4, 0.1)
+    tvec = OpenCVHelp.translationToTVec([trl])
+    result = OpenCVHelp.tVecToTranslation(tvec[0])
+
+    assert result.X() == pytest.approx(trl.X(), 0.005)
+    assert result.Y() == pytest.approx(trl.Y(), 0.005)
+    assert result.Z() == pytest.approx(trl.Z(), 0.005)
+
+
+def test_RotConvert():
+    rot = Rotation3d(0.5, 1, -1)
+    rvec = OpenCVHelp.rotationToRVec(rot)
+    result = OpenCVHelp.rVecToRotation(rvec[0])
+
+    assert result.X() == pytest.approx(rot.X(), 0.25)
+    assert result.Y() == pytest.approx(rot.Y(), 0.25)
+    assert result.Z() == pytest.approx(rot.Z(), 0.25)
+
+
+def test_Projection():
+    prop = SimCameraProperties()
+
+    target = VisionTargetSim(
+        Pose3d(Translation3d(1.0, 0.0, 0.0), Rotation3d(0.0, 0.0, math.pi)),
+        TargetModel.AprilTag16h5(),
+        4774,
+    )
+
+    cameraPose = Pose3d(Translation3d(), Rotation3d())
+
+    camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
+    imagePoints = OpenCVHelp.projectPoints(
+        prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices()
+    )
+
+    # find circulation (counter/clockwise-ness)
+    circulation = 0.0
+    for i in range(0, len(imagePoints)):
+        xDiff = imagePoints[(i + 1) % 4][0][0] - imagePoints[i][0][0]
+        ySum = imagePoints[(i + 1) % 4][0][1] + imagePoints[i][0][1]
+        circulation += xDiff * ySum
+
+    assert circulation > 0, "2d fiducial points aren't counter-clockwise"
+
+    # TODO Uncomment after OpenCVHelp.undistortPoints is implemented
+    # # undo projection distortion
+    # imagePoints = OpenCVHelp.undistortPoints(
+    #     prop.getIntrinsics(), prop.getDistCoeffs(), imagePoints
+    # )
+    # # test projection results after moving camera
+    # avgCenterRot1 = prop.getPixelRot(OpenCVHelp.avgPoint(imagePoints))
+    # cameraPose = cameraPose + Transform3d(Translation3d(), Rotation3d(0.0, 0.25, 0.25))
+    # camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
+    # imagePoints = OpenCVHelp.projectPoints(
+    #     prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices()
+    # )
+    # avgCenterRot2 = prop.getPixelRot(OpenCVHelp.avgPoint(imagePoints))
+
+    # yaw2d = Rotation2d(avgCenterRot2.Z())
+    # pitch2d = Rotation2d(avgCenterRot2.Y())
+    # yawDiff = yaw2d - Rotation2d(avgCenterRot1.Z())
+    # pitchDiff = pitch2d - Rotation2d(avgCenterRot2.Y())
+    # assert yawDiff.radians() < 0.0, "2d points don't follow yaw"
+    # assert pitchDiff.radians() < 0.0, "2d points don't follow pitch"
+
+    # actualRelation = CameraTargetRelation(cameraPose, target.getPose())
+
+    # assert actualRelation.camToTargPitch.degrees() == pytest.approx(
+    #     pitchDiff.degrees()
+    #     * math.cos(yaw2d.radians()),  # adjust for unaccounted perspective distortion
+    #     abs=0.25,
+    # ), "2d pitch doesn't match 3d"
+    # assert actualRelation.camToTargYaw.degrees() == pytest.approx(
+    #     yawDiff.degrees(), abs=0.25
+    # ), "2d yaw doesn't match 3d"
+
+
+def test_SolvePNP_SQUARE():
+    prop = SimCameraProperties()
+
+    # square AprilTag target
+    target = VisionTargetSim(
+        Pose3d(Translation3d(5.0, 0.5, 1.0), Rotation3d(0.0, 0.0, math.pi)),
+        TargetModel.AprilTag16h5(),
+        4774,
+    )
+    cameraPose = Pose3d(Translation3d(), Rotation3d())
+    camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
+
+    # target relative to camera
+    relTarget = camRt.applyPose(target.getPose())
+
+    # simulate solvePNP estimation
+    targetCorners = OpenCVHelp.projectPoints(
+        prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices()
+    )
+
+    pnpSim = OpenCVHelp.solvePNP_Square(
+        prop.getIntrinsics(),
+        prop.getDistCoeffs(),
+        target.getModel().vertices,
+        targetCorners,
+    )
+
+    assert pnpSim is not None
+
+    # check solvePNP estimation accuracy
+    assert relTarget.rotation().X() == pytest.approx(
+        pnpSim.best.rotation().X(), abs=0.25
+    )
+    assert relTarget.rotation().Y() == pytest.approx(
+        pnpSim.best.rotation().Y(), abs=0.25
+    )
+    assert relTarget.rotation().Z() == pytest.approx(
+        pnpSim.best.rotation().Z(), abs=0.25
+    )
+
+    assert relTarget.translation().X() == pytest.approx(
+        pnpSim.best.translation().X(), abs=0.005
+    )
+    assert relTarget.translation().Y() == pytest.approx(
+        pnpSim.best.translation().Y(), abs=0.005
+    )
+    assert relTarget.translation().Z() == pytest.approx(
+        pnpSim.best.translation().Z(), abs=0.005
+    )
+
+
+def test_SolvePNP_SQPNP():
+    prop = SimCameraProperties()
+
+    # (for targets with arbitrary number of non-colinear points > 2)
+    target = VisionTargetSim(
+        Pose3d(Translation3d(5.0, 0.5, 1.0), Rotation3d(0.0, 0.0, math.pi)),
+        TargetModel(
+            verts=[
+                Translation3d(0.0, 0.0, 0.0),
+                Translation3d(1.0, 0.0, 0.0),
+                Translation3d(0.0, 1.0, 0.0),
+                Translation3d(0.0, 0.0, 1.0),
+                Translation3d(0.125, 0.25, 0.5),
+                Translation3d(0.0, 0.0, -1.0),
+                Translation3d(0.0, -1.0, 0.0),
+                Translation3d(-1.0, 0.0, 0.0),
+            ]
+        ),
+        4774,
+    )
+    cameraPose = Pose3d(Translation3d(), Rotation3d())
+    camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
+    # target relative to camera
+    relTarget = camRt.applyPose(target.getPose())
+
+    # simulate solvePNP estimation
+    targetCorners = OpenCVHelp.projectPoints(
+        prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices()
+    )
+
+    pnpSim = OpenCVHelp.solvePNP_SQPNP(
+        prop.getIntrinsics(),
+        prop.getDistCoeffs(),
+        target.getModel().vertices,
+        targetCorners,
+    )
+
+    assert pnpSim is not None
+
+    # check solvePNP estimation accuracy
+    assert relTarget.rotation().X() == pytest.approx(
+        pnpSim.best.rotation().X(), abs=0.25
+    )
+    assert relTarget.rotation().Y() == pytest.approx(
+        pnpSim.best.rotation().Y(), abs=0.25
+    )
+    assert relTarget.rotation().Z() == pytest.approx(
+        pnpSim.best.rotation().Z(), abs=0.25
+    )
+
+    assert relTarget.translation().X() == pytest.approx(
+        pnpSim.best.translation().X(), abs=0.005
+    )
+    assert relTarget.translation().Y() == pytest.approx(
+        pnpSim.best.translation().Y(), abs=0.005
+    )
+    assert relTarget.translation().Z() == pytest.approx(
+        pnpSim.best.translation().Z(), abs=0.005
+    )

photon-lib/py/test/photonCamera_test.py

@@ -0,0 +1,36 @@
+import pytest
+from photonlibpy import Packet
+from photonlibpy.targeting import PhotonPipelineResult
+
+
+@pytest.fixture(autouse=True)
+def setupCommon() -> None:
+    pass
+
+
+def test_Empty() -> None:
+    packet = Packet(b"1")
+    PhotonPipelineResult()
+    packet.setData(bytes(0))
+    PhotonPipelineResult.photonStruct.unpack(packet)
+    # There is no need for an assert as we are checking
+    # if this throws an exception (it should not)
+
+
+@pytest.mark.parametrize(
+    "robotStart, coprocStart, robotRestart, coprocRestart",
+    [
+        [1, 10, 30, 30],
+        [10, 2, 30, 30],
+        [10, 10, 30, 30],
+        # Reboot just the robot
+        [1, 1, 10, 30],
+        # Reboot just the coproc
+        [1, 1, 30, 10],
+    ],
+)
+def test_RestartingRobotandCoproc(
+    robotStart: int, coprocStart: int, robotRestart: int, coprocRestart: int
+):
+    # Python doesn't have a TimeSyncClient so we can't run this yet
+    pass

photon-lib/py/test/simCameraProperties_test.py

@@ -0,0 +1,105 @@
+import math
+
+import numpy as np
+import pytest
+from photonlibpy.estimation import RotTrlTransform3d
+from photonlibpy.simulation import SimCameraProperties
+from wpimath.geometry import Rotation2d, Translation3d
+
+
+@pytest.fixture(autouse=True)
+def scp() -> SimCameraProperties:
+    props = SimCameraProperties()
+    props.setCalibration(1000, 1000, fovDiag=Rotation2d(math.radians(90.0)))
+    return props
+
+
+def test_Constructor() -> None:
+    SimCameraProperties()
+    with pytest.raises(Exception):
+        SimCameraProperties("4774")
+
+
+def test_GetPixelYaw(scp) -> None:
+    rot = scp.getPixelYaw(scp.getResWidth() / 2)
+    assert rot.degrees() == pytest.approx(0.0, abs=1.0)
+    rot = scp.getPixelYaw(0.0)
+    # FOV is square
+    assert rot.degrees() == pytest.approx(45.0 / math.sqrt(2.0), abs=5.0)
+    rot = scp.getPixelYaw(scp.getResWidth())
+    assert rot.degrees() == pytest.approx(-45.0 / math.sqrt(2.0), abs=5.0)
+
+
+def test_GetPixelPitch(scp) -> None:
+    rot = scp.getPixelPitch(scp.getResHeight() / 2)
+    assert rot.degrees() == pytest.approx(0.0, abs=1.0)
+    rot = scp.getPixelPitch(0.0)
+    # FOV is square
+    assert rot.degrees() == pytest.approx(-45.0 / math.sqrt(2.0), abs=5.0)
+    rot = scp.getPixelPitch(scp.getResHeight())
+    assert rot.degrees() == pytest.approx(45.0 / math.sqrt(2.0), abs=5.0)
+
+
+def test_GetPixelRot(scp) -> None:
+    rot = scp.getPixelRot(np.array([scp.getResWidth() / 2.0, scp.getResHeight() / 2.0]))
+    assert rot.x_degrees == pytest.approx(0.0, abs=5)
+    assert rot.y_degrees == pytest.approx(0.0, abs=5)
+    assert rot.z_degrees == pytest.approx(0.0, abs=5)
+    rot = scp.getPixelRot(np.array([0.0, 0.0]))
+    assert rot.x_degrees == pytest.approx(0.0, abs=5)
+    assert rot.y_degrees == pytest.approx(-45.0 / math.sqrt(2.0), abs=5)
+    assert rot.z_degrees == pytest.approx(45.0 / math.sqrt(2.0), abs=5)
+    rot = scp.getPixelRot(np.array([scp.getResWidth(), scp.getResHeight()]))
+    assert rot.x_degrees == pytest.approx(0.0, abs=5)
+    assert rot.y_degrees == pytest.approx(45.0 / math.sqrt(2.0), abs=5)
+    assert rot.z_degrees == pytest.approx(-45.0 / math.sqrt(2.0), abs=5)
+
+
+def test_GetCorrectedPixelRot(scp) -> None:
+    rot = scp.getCorrectedPixelRot(
+        np.array([scp.getResWidth() / 2.0, scp.getResHeight() / 2.0])
+    )
+    assert rot.x_degrees == pytest.approx(0.0, abs=5)
+    assert rot.y_degrees == pytest.approx(0.0, abs=5)
+    assert rot.z_degrees == pytest.approx(0.0, abs=5)
+    rot = scp.getCorrectedPixelRot(np.array([0.0, 0.0]))
+    assert rot.x_degrees == pytest.approx(0.0, abs=5)
+    assert rot.y_degrees == pytest.approx(-45.0 / math.sqrt(2.0), abs=5)
+    assert rot.z_degrees == pytest.approx(45.0 / math.sqrt(2.0), abs=5)
+    rot = scp.getCorrectedPixelRot(np.array([scp.getResWidth(), scp.getResHeight()]))
+    assert rot.x_degrees == pytest.approx(0.0, abs=5)
+    assert rot.y_degrees == pytest.approx(45.0 / math.sqrt(2.0), abs=5)
+    assert rot.z_degrees == pytest.approx(-45.0 / math.sqrt(2.0), abs=5)
+
+
+def test_GetVisibleLine(scp) -> None:
+    camRt = RotTrlTransform3d()
+    a = Translation3d()
+    b = Translation3d()
+    retval = scp.getVisibleLine(camRt, a, b)
+    assert retval == (None, None)
+
+    a = Translation3d(-5.0, -0.1, 0)
+    b = Translation3d(5.0, 0.1, 0)
+    retval = scp.getVisibleLine(camRt, a, b)
+    assert retval == (0.5, 0.5)
+
+
+def test_EstPixelNoise(scp) -> None:
+    with pytest.raises(Exception):
+        scp.test_EstPixelNoise(np.array([0, 0]))
+    with pytest.raises(Exception):
+        scp.test_EstPixelNoise(np.array([[0], [0]]))
+
+    pts = np.array([[[0, 0]], [[0, 0]]])
+
+    # No noise parameters set
+    noisy = scp.estPixelNoise(pts)
+    for n, p in zip(noisy, pts):
+        assert n.all() == p.all()
+
+    # Noise parameters set
+    scp.setCalibError(1.0, 1.0)
+    noisy = scp.estPixelNoise(pts)
+    for n, p in zip(noisy, pts):
+        assert n.any() != p.any()

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)