photonlibpy 2025.3.2__py3-none-any.whl → 2026.0.0a1__py3-none-any.whl

photonlibpy/photonCamera.py

@@ -18,6 +18,7 @@
 from enum import Enum
 from typing import List
 
+import hal
 import ntcore
 
 # magical import to make serde stuff work
@@ -48,6 +49,8 @@ def setVersionCheckEnabled(enabled: bool):
 
 
 class PhotonCamera:
+    instance_count = 1
+
     def __init__(self, cameraName: str):
         """Constructs a PhotonCamera from the name of the camera.
 
@@ -108,6 +111,13 @@ class PhotonCamera:
         # Start the time sync server
         inst.start()
 
+        # Usage reporting
+        hal.report(
+            hal.tResourceType.kResourceType_PhotonCamera.value,
+            PhotonCamera.instance_count,
+        )
+        PhotonCamera.instance_count += 1
+
     def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
         """
         The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().

photonlibpy/photonPoseEstimator.py

@@ -18,9 +18,20 @@
 import enum
 from typing import Optional
 
+import hal
 import wpilib
+import wpimath.units
 from robotpy_apriltag import AprilTagFieldLayout
-from wpimath.geometry import Pose2d, Pose3d, Transform3d
+from wpimath.geometry import (
+    Pose2d,
+    Pose3d,
+    Rotation2d,
+    Rotation3d,
+    Transform3d,
+    Translation2d,
+    Translation3d,
+)
+from wpimath.interpolation import TimeInterpolatableRotation2dBuffer
 
 from .estimatedRobotPose import EstimatedRobotPose
 from .photonCamera import PhotonCamera
@@ -59,8 +70,21 @@ class PoseStrategy(enum.Enum):
     This runs on the RoboRIO, and can take a lot of time.
     """
 
+    PNP_DISTANCE_TRIG_SOLVE = enum.auto()
+    """
+    Use distance data from best visible tag to compute a Pose. This runs on
+    the RoboRIO in order to access the robot's yaw heading, and MUST have
+    addHeadingData called every frame so heading data is up-to-date.
+
+    Produces a Pose2d in estimatedRobotPose (0 for z, roll, pitch).
+
+    See https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
+    """
+
 
 class PhotonPoseEstimator:
+    instance_count = 1
+
     """
     The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
     given timestamp on the field to produce a single robot in field pose, using the strategy set
@@ -95,8 +119,14 @@ class PhotonPoseEstimator:
         self._poseCacheTimestampSeconds = -1.0
         self._lastPose: Optional[Pose3d] = None
         self._referencePose: Optional[Pose3d] = None
+        self._headingBuffer = TimeInterpolatableRotation2dBuffer(1)
 
-        # TODO: Implement HAL reporting
+        # Usage reporting
+        hal.report(
+            hal.tResourceType.kResourceType_PhotonPoseEstimator.value,
+            PhotonPoseEstimator.instance_count,
+        )
+        PhotonPoseEstimator.instance_count += 1
 
     @property
     def fieldTags(self) -> AprilTagFieldLayout:
@@ -199,9 +229,35 @@ class PhotonPoseEstimator:
         self._poseCacheTimestampSeconds = -1.0
 
     def _checkUpdate(self, oldObj, newObj) -> None:
-        if oldObj != newObj and oldObj is not None and oldObj is not newObj:
+        if oldObj != newObj:
             self._invalidatePoseCache()
 
+    def addHeadingData(
+        self, timestampSeconds: wpimath.units.seconds, heading: Rotation2d | Rotation3d
+    ) -> None:
+        """
+        Add robot heading data to buffer. Must be called periodically for the **PNP_DISTANCE_TRIG_SOLVE** strategy.
+
+        :param timestampSeconds :timestamp of the robot heading data
+        :param heading: field-relative robot heading at given timestamp
+        """
+        if isinstance(heading, Rotation3d):
+            heading = heading.toRotation2d()
+        self._headingBuffer.addSample(timestampSeconds, heading)
+
+    def resetHeadingData(
+        self, timestampSeconds: wpimath.units.seconds, heading: Rotation2d | Rotation3d
+    ) -> None:
+        """
+        Clears all heading data in the buffer, and adds a new seed. Useful for preventing estimates
+        from utilizing heading data provided prior to a pose or rotation reset.
+
+        :param timestampSeconds: timestamp of the robot heading data
+        :param  heading: field-relative robot heading at given timestamp
+        """
+        self._headingBuffer.clear()
+        self.addHeadingData(timestampSeconds, heading)
+
     def update(
         self, cameraResult: Optional[PhotonPipelineResult] = None
     ) -> Optional[EstimatedRobotPose]:
@@ -255,6 +311,8 @@ class PhotonPoseEstimator:
             estimatedPose = self._lowestAmbiguityStrategy(cameraResult)
         elif strat is PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR:
             estimatedPose = self._multiTagOnCoprocStrategy(cameraResult)
+        elif strat is PoseStrategy.PNP_DISTANCE_TRIG_SOLVE:
+            estimatedPose = self._pnpDistanceTrigSolveStrategy(cameraResult)
         else:
             wpilib.reportError(
                 "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", False
@@ -266,6 +324,52 @@ class PhotonPoseEstimator:
 
         return estimatedPose
 
+    def _pnpDistanceTrigSolveStrategy(
+        self, result: PhotonPipelineResult
+    ) -> Optional[EstimatedRobotPose]:
+        if (bestTarget := result.getBestTarget()) is None:
+            return None
+
+        if (
+            headingSample := self._headingBuffer.sample(result.getTimestampSeconds())
+        ) is None:
+            return None
+
+        if (tagPose := self._fieldTags.getTagPose(bestTarget.fiducialId)) is None:
+            return None
+
+        camToTagTranslation = (
+            Translation3d(
+                bestTarget.getBestCameraToTarget().translation().norm(),
+                Rotation3d(
+                    0,
+                    -wpimath.units.degreesToRadians(bestTarget.getPitch()),
+                    -wpimath.units.degreesToRadians(bestTarget.getYaw()),
+                ),
+            )
+            .rotateBy(self.robotToCamera.rotation())
+            .toTranslation2d()
+            .rotateBy(headingSample)
+        )
+
+        fieldToCameraTranslation = (
+            tagPose.toPose2d().translation() - camToTagTranslation
+        )
+        camToRobotTranslation: Translation2d = -(
+            self.robotToCamera.translation().toTranslation2d()
+        )
+        camToRobotTranslation = camToRobotTranslation.rotateBy(headingSample)
+        robotPose = Pose2d(
+            fieldToCameraTranslation + camToRobotTranslation, headingSample
+        )
+
+        return EstimatedRobotPose(
+            Pose3d(robotPose),
+            result.getTimestampSeconds(),
+            result.getTargets(),
+            PoseStrategy.PNP_DISTANCE_TRIG_SOLVE,
+        )
+
     def _multiTagOnCoprocStrategy(
         self, result: PhotonPipelineResult
     ) -> Optional[EstimatedRobotPose]:

photonlibpy/simulation/photonCameraSim.py

@@ -420,14 +420,15 @@ class PhotonCameraSim:
 
         # put this simulated data to NT
         self.heartbeatCounter += 1
-        now_micros = wpilib.Timer.getFPGATimestamp() * 1e6
+        publishTimestampMicros = wpilib.Timer.getFPGATimestamp() * 1e6
         return PhotonPipelineResult(
+            ntReceiveTimestampMicros=int(publishTimestampMicros + 10),
             metadata=PhotonPipelineMetadata(
-                int(now_micros - latency * 1e6),
-                int(now_micros),
-                self.heartbeatCounter,
+                captureTimestampMicros=int(publishTimestampMicros - latency * 1e6),
+                publishTimestampMicros=int(publishTimestampMicros),
+                sequenceID=self.heartbeatCounter,
                 # Pretend like we heard a pong recently
-                int(np.random.uniform(950, 1050)),
+                timeSinceLastPong=int(np.random.uniform(950, 1050)),
             ),
             targets=detectableTgts,
             multitagResult=multiTagResults,

photonlibpy/targeting/photonPipelineResult.py

@@ -47,13 +47,10 @@ class PhotonPipelineResult:
         timestamp, coproc timebase))
         """
         # TODO - we don't trust NT4 to correctly latency-compensate ntReceiveTimestampMicros
-        return (
-            self.ntReceiveTimestampMicros
-            - (
-                self.metadata.publishTimestampMicros
-                - self.metadata.captureTimestampMicros
-            )
-        ) / 1e6
+        latency = (
+            self.metadata.publishTimestampMicros - self.metadata.captureTimestampMicros
+        )
+        return (self.ntReceiveTimestampMicros - latency) / 1e6
 
     def getTargets(self) -> list[PhotonTrackedTarget]:
         return self.targets

photonlibpy/version.py

@@ -1,2 +1,2 @@
-PHOTONLIB_VERSION="2025.3.2"
-PHOTONVISION_VERSION="v2025.3.2"
+PHOTONLIB_VERSION="v2026.0.0.alpha.1"
+PHOTONVISION_VERSION="v2026.0.0-alpha-1"

{photonlibpy-2025.3.2.dist-info → photonlibpy-2026.0.0a1.dist-info}/METADATA

@@ -1,7 +1,7 @@
 Metadata-Version: 2.1
 Name: photonlibpy
-Version: 2025.3.2
-Summary: Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version v2025.3.2 .
+Version: 2026.0.0a1
+Summary: Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version v2026.0.0-alpha-1 .
 Home-page: https://photonvision.org
 Author: Photonvision Development Team
 Classifier: License :: OSI Approved :: MIT License

{photonlibpy-2025.3.2.dist-info → photonlibpy-2026.0.0a1.dist-info}/RECORD

@@ -1,10 +1,10 @@
 photonlibpy/__init__.py,sha256=nPUL143Q0VAdgE4L1U-xRkodvoU4a76w5lu_1iDVJjI,1413
 photonlibpy/estimatedRobotPose.py,sha256=X7wF9xdPXGKSVy0MY0qrWZJOEbuZPd721lYp0KXKlP0,1603
 photonlibpy/packet.py,sha256=5YomViVFwOljL2FGOetWM9FbPc_yCQ15ylzkYlgLIs8,9724
-photonlibpy/photonCamera.py,sha256=r5dH2S4ZcwiBDrpK58aHULrifIA0Tqr4iJp94My75A0,13006
-photonlibpy/photonPoseEstimator.py,sha256=2iMqxPFsQHTsq95yv-WCSv1a6wXNcHPqOyMc4Bu6IG0,12584
+photonlibpy/photonCamera.py,sha256=Vkn82MOKbWCLuTU8ljXS-yVQfhoJjCBcaJsbluhlQxQ,13244
+photonlibpy/photonPoseEstimator.py,sha256=OBO5eVqz8UVlpu1R3YHcFLBAPfZfqqAa6pvBdb4oiq8,16298
 photonlibpy/py.typed,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
-photonlibpy/version.py,sha256=SAchXEI4wYx6RIgKL3e_khs4gaDfnKLfyzylwj_RIgk,62
+photonlibpy/version.py,sha256=r9LHo7JkyIMrKEwnwnZ8dzRLsJEufYcwODdFOZnEhiY,79
 photonlibpy/estimation/__init__.py,sha256=pZ-d6fN1DJvT-lRl4FfIos5HAvlzmetIOrGIinrdv7k,223
 photonlibpy/estimation/cameraTargetRelation.py,sha256=i7DPBXtkZve4ToXQscEIe-5F1oGQ1Qmf5QBaE__EeMQ,1158
 photonlibpy/estimation/openCVHelp.py,sha256=O1dV7v7RHSyw7l5L0QXbal6t9K7iyvEG76tG-t4AFVg,12388
@@ -21,7 +21,7 @@ photonlibpy/generated/__init__.py,sha256=mElM8M88---wxTWO-SRqIJ4EfxN0fdIUwZBZ-UI
 photonlibpy/networktables/NTTopicSet.py,sha256=29wPgXcuqT-u72-YXwSjRHWhECNzU8eDsexcqlA8KQ0,2967
 photonlibpy/networktables/__init__.py,sha256=o_LxTdyIylAszMy_zhUtTkXHyu6jqxccccj78d44OrI,35
 photonlibpy/simulation/__init__.py,sha256=HazsBMXg1HT8TnyxYO8QI9NXwZOrtuCSytnTdquLBKw,358
-photonlibpy/simulation/photonCameraSim.py,sha256=24AcbAjWEfGBAhuIXuTuISb9uM0lLtgMdqSGZvJGPjg,19992
+photonlibpy/simulation/photonCameraSim.py,sha256=A4cTSZ87PIg24Dim63I_DaP1SJ4h9bDR5LIySoyClr4,20174
 photonlibpy/simulation/simCameraProperties.py,sha256=ODVxnylF8zw9HZSbw0PzG_OEtUo9ChRo-G_iEgADOCg,27195
 photonlibpy/simulation/videoSimUtil.py,sha256=xMuTvJ2Jx9IoQqmAJi_zUm06MdEwhVpIz9OyzYQp0k4,29
 photonlibpy/simulation/visionSystemSim.py,sha256=GmKs0d32WE8B020YEWnj-0dQuCnVv1ScGdcFl1fOsKo,13835
@@ -29,11 +29,19 @@ photonlibpy/simulation/visionTargetSim.py,sha256=FH85fKE4NntowUvssfgZ1KlE-I_3Z-Q
 photonlibpy/targeting/TargetCorner.py,sha256=ouKj3E5uD76OZSNHHuSDzKOY65a8HqtcOsuejH-MVsU,276
 photonlibpy/targeting/__init__.py,sha256=OxxkBvBa6sFdjG7T1hO8CwBkRHk6GYdXbVhVgYfW7Gc,402
 photonlibpy/targeting/multiTargetPNPResult.py,sha256=Y9rweHtMzoCZ6mv6F8CutQi2Thq5pHN0ydBWvTCsOwY,806
-photonlibpy/targeting/photonPipelineResult.py,sha256=MbaSyHZTJpoKTtLOZztpSGSt9xWWFqhzgwj8medObVA,2732
+photonlibpy/targeting/photonPipelineResult.py,sha256=H_wsIQfdo41xNT32YqfttH6AYQRZhAMR7XDiysMa3lw,2700
 photonlibpy/targeting/photonTrackedTarget.py,sha256=zCoFp32hX-3GmBYEmsYBQieBoMzXtP2F_55_q0zPOXA,1956
 photonlibpy/timesync/__init__.py,sha256=pECFraHRZC2Dl4x4OrmhW8oWkawP68pCb-Y61dZycJs,25
 photonlibpy/timesync/timeSyncServer.py,sha256=0d76uHBS_Jo3etvougdZHapbRrbb31iu-jlf8h9U_QQ,3194
-photonlibpy-2025.3.2.dist-info/METADATA,sha256=I2o8AX_j0PMkvUCz4lawZ2jWyupx2Gjk_86XymhBa3U,721
-photonlibpy-2025.3.2.dist-info/WHEEL,sha256=tZoeGjtWxWRfdplE7E3d45VPlLNQnvbKiYnx7gwAy8A,92
-photonlibpy-2025.3.2.dist-info/top_level.txt,sha256=T8Xc6U6he2VjKUAca6zawSkHdUZuLanxYIc4nxw2ctc,12
-photonlibpy-2025.3.2.dist-info/RECORD,,
+test/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+test/openCVHelp_test.py,sha256=Lr6yDXB5dcwWNoOf0azTizs3F_uarCwP92lKfMIwKXo,6996
+test/photonCamera_test.py,sha256=pdT7AplfBOuRK8pQVvO2dSCAwEhpaLU1JrIxzRODvpY,921
+test/photonPoseEstimator_test.py,sha256=FUyIF2y-tfQEJqsCXCjqa_YVNU19NsdRVowziwjR5eI,13024
+test/photonlibpy_test.py,sha256=u_Ycke7fCfV2sr7O9RBWD8e1w-XWdMAx01eMJKIcbYM,1565
+test/simCameraProperties_test.py,sha256=vJJjNmKoO_neFIIgRW8KRro75s7VPOvkekex27oW6aU,3763
+test/testUtil.py,sha256=sxACyosw9IjfTlZAjrsSKr0mCBQUjC3tvAQUqndGtkk,2297
+test/visionSystemSim_test.py,sha256=8lBJD6vdqV2LwgNYtEgrJjuizPaRRJ2O4iGKhEeIDkM,21007
+photonlibpy-2026.0.0a1.dist-info/METADATA,sha256=t6pxUB-RbN5PvqxloLYeHhK2S5Dm_CWXHYqNNM4cTVk,731
+photonlibpy-2026.0.0a1.dist-info/WHEEL,sha256=tZoeGjtWxWRfdplE7E3d45VPlLNQnvbKiYnx7gwAy8A,92
+photonlibpy-2026.0.0a1.dist-info/top_level.txt,sha256=f7vWroLAnhnbYyix-BbJXaIzvsfWUu7sMlKZrDISjHE,17
+photonlibpy-2026.0.0a1.dist-info/RECORD,,

{photonlibpy-2025.3.2.dist-info → photonlibpy-2026.0.0a1.dist-info}/top_level.txt

@@ -1 +1,2 @@
 photonlibpy
+test

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.createArbitrary(
+            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
+    )

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