photonlibpy 2024.2.7__tar.gz → 2026.1.1rc3__tar.gz

photonlibpy-2026.1.1rc3/PKG-INFO

@@ -0,0 +1,24 @@
+Metadata-Version: 2.4
+Name: photonlibpy
+Version: 2026.1.1rc3
+Summary: Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version v2026.1.1-rc-3 .
+Home-page: https://photonvision.org
+Author: Photonvision Development Team
+Classifier: License :: OSI Approved :: MIT License
+Description-Content-Type: text/markdown
+Requires-Dist: numpy~=2.3
+Requires-Dist: wpilib==2026.1.1
+Requires-Dist: robotpy-wpimath==2026.1.1
+Requires-Dist: robotpy-apriltag==2026.1.1
+Requires-Dist: robotpy-cscore==2026.1.1
+Requires-Dist: pyntcore==2026.1.1
+Requires-Dist: opencv-python; platform_machine != "roborio"
+Dynamic: author
+Dynamic: classifier
+Dynamic: description
+Dynamic: description-content-type
+Dynamic: home-page
+Dynamic: requires-dist
+Dynamic: summary
+
+A Pure-python implementation of PhotonLib

photonlibpy-2026.1.1rc3/photonlibpy/__init__.py

@@ -0,0 +1,35 @@
+#
+# MIT License
+#
+# Copyright (c) PhotonVision
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+#
+
+from .estimatedRobotPose import EstimatedRobotPose
+from .packet import Packet
+from .photonCamera import PhotonCamera
+from .photonPoseEstimator import PhotonPoseEstimator
+
+__all__ = (
+    "EstimatedRobotPose",
+    "Packet",
+    "PhotonCamera",
+    "PhotonPoseEstimator",
+)

photonlibpy-2026.1.1rc3/photonlibpy/estimatedRobotPose.py

@@ -0,0 +1,36 @@
+###############################################################################
+## Copyright (C) Photon Vision.
+###############################################################################
+## This program is free software: you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+## GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this program.  If not, see <https://www.gnu.org/licenses/>.
+###############################################################################
+
+from dataclasses import dataclass
+
+from wpimath.geometry import Pose3d
+
+from .targeting.photonTrackedTarget import PhotonTrackedTarget
+
+
+@dataclass
+class EstimatedRobotPose:
+    """An estimated pose based on pipeline result"""
+
+    estimatedPose: Pose3d
+    """The estimated pose"""
+
+    timestampSeconds: float
+    """The estimated time the frame used to derive the robot pose was taken"""
+
+    targetsUsed: list[PhotonTrackedTarget]
+    """A list of the targets used to compute this pose"""

photonlibpy-2026.1.1rc3/photonlibpy/estimation/__init__.py

@@ -0,0 +1,5 @@
+from .cameraTargetRelation import CameraTargetRelation
+from .openCVHelp import OpenCVHelp
+from .rotTrlTransform3d import RotTrlTransform3d
+from .targetModel import TargetModel
+from .visionEstimation import VisionEstimation

photonlibpy-2026.1.1rc3/photonlibpy/estimation/cameraTargetRelation.py

@@ -0,0 +1,25 @@
+import math
+
+from wpimath.geometry import Pose3d, Rotation2d, Transform3d
+from wpimath.units import meters
+
+
+class CameraTargetRelation:
+    def __init__(self, cameraPose: Pose3d, targetPose: Pose3d):
+        self.camPose = cameraPose
+        self.camToTarg = Transform3d(cameraPose, targetPose)
+        self.camToTargDist = self.camToTarg.translation().norm()
+        self.camToTargDistXY: meters = math.hypot(
+            self.camToTarg.translation().X(), self.camToTarg.translation().Y()
+        )
+        self.camToTargYaw = Rotation2d(self.camToTarg.X(), self.camToTarg.Y())
+        self.camToTargPitch = Rotation2d(self.camToTargDistXY, -self.camToTarg.Z())
+        self.camToTargAngle = Rotation2d(
+            math.hypot(self.camToTargYaw.radians(), self.camToTargPitch.radians())
+        )
+        self.targToCam = Transform3d(targetPose, cameraPose)
+        self.targToCamYaw = Rotation2d(self.targToCam.X(), self.targToCam.Y())
+        self.targToCamPitch = Rotation2d(self.camToTargDistXY, -self.targToCam.Z())
+        self.targtoCamAngle = Rotation2d(
+            math.hypot(self.targToCamYaw.radians(), self.targToCamPitch.radians())
+        )

photonlibpy-2026.1.1rc3/photonlibpy/estimation/openCVHelp.py

@@ -0,0 +1,314 @@
+import logging
+import math
+from typing import Any
+
+import cv2 as cv
+import numpy as np
+from wpimath.geometry import Rotation3d, Transform3d, Translation3d
+
+from ..targeting import PnpResult, TargetCorner
+from .rotTrlTransform3d import RotTrlTransform3d
+
+NWU_TO_EDN = Rotation3d(np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]]))
+EDN_TO_NWU = Rotation3d(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]]))
+
+logger = logging.getLogger(__name__)
+
+
+class OpenCVHelp:
+    @staticmethod
+    def getMinAreaRect(points: np.ndarray) -> cv.RotatedRect:
+        return cv.RotatedRect(*cv.minAreaRect(points))
+
+    @staticmethod
+    def translationNWUtoEDN(trl: Translation3d) -> Translation3d:
+        return trl.rotateBy(NWU_TO_EDN)
+
+    @staticmethod
+    def rotationNWUtoEDN(rot: Rotation3d) -> Rotation3d:
+        return -NWU_TO_EDN + (rot + NWU_TO_EDN)
+
+    @staticmethod
+    def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
+        """Creates a new :class:`np.array` with these 3d translations. The opencv tvec is a vector with
+        three elements representing {x, y, z} in the EDN coordinate system.
+
+        :param translations: The translations to convert into a np.array
+        """
+
+        retVal: list[list] = []
+        for translation in translations:
+            trl = OpenCVHelp.translationNWUtoEDN(translation)
+            retVal.append([trl.X(), trl.Y(), trl.Z()])
+        return np.array(
+            retVal,
+            dtype=np.float32,
+        )
+
+    @staticmethod
+    def rotationToRVec(rotation: Rotation3d) -> np.ndarray:
+        """Creates a new :class:`.np.array` with this 3d rotation. The opencv rvec Mat is a vector with
+        three elements representing the axis scaled by the angle in the EDN coordinate system. (angle =
+        norm, and axis = rvec / norm)
+
+        :param rotation: The rotation to convert into a np.array
+        """
+
+        retVal: list[np.ndarray] = []
+        rot = OpenCVHelp.rotationNWUtoEDN(rotation)
+        rotVec = rot.getQuaternion().toRotationVector()
+        retVal.append(rotVec)
+        return np.array(
+            retVal,
+            dtype=np.float32,
+        )
+
+    @staticmethod
+    def avgPoint(points: np.ndarray) -> np.ndarray:
+        x = 0.0
+        y = 0.0
+        for p in points:
+            x += p[0, 0]
+            y += p[0, 1]
+        return np.array([[x / len(points), y / len(points)]])
+
+    @staticmethod
+    def pointsToTargetCorners(points: np.ndarray) -> list[TargetCorner]:
+        corners = [TargetCorner(p[0, 0], p[0, 1]) for p in points]
+        return corners
+
+    @staticmethod
+    def cornersToPoints(corners: list[TargetCorner]) -> np.ndarray:
+        points = [[[c.x, c.y]] for c in corners]
+        return np.array(points)
+
+    @staticmethod
+    def projectPoints(
+        cameraMatrix: np.ndarray,
+        distCoeffs: np.ndarray,
+        camRt: RotTrlTransform3d,
+        objectTranslations: list[Translation3d],
+    ) -> np.ndarray:
+        objectPoints = OpenCVHelp.translationToTVec(objectTranslations)
+        rvec = OpenCVHelp.rotationToRVec(camRt.getRotation())
+        tvec = OpenCVHelp.translationToTVec(
+            [
+                camRt.getTranslation(),
+            ]
+        )
+
+        pts, _ = cv.projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs)
+        return pts
+
+    @staticmethod
+    def reorderCircular(
+        elements: list[Any] | np.ndarray, backwards: bool, shiftStart: int
+    ) -> list[Any]:
+        """Reorders the list, optionally indexing backwards and wrapping around to the last element after
+        the first, and shifting all indices in the direction of indexing.
+
+        e.g.
+
+        ({1,2,3}, false, 1) == {2,3,1}
+
+        ({1,2,3}, true, 0) == {1,3,2}
+
+        ({1,2,3}, true, 1) == {3,2,1}
+
+        :param elements:   list elements
+        :param backwards:  If indexing should happen in reverse (0, size-1, size-2, ...)
+        :param shiftStart: How much the initial index should be shifted (instead of starting at index 0,
+                           start at shiftStart, negated if backwards)
+
+        :returns: Reordered list
+        """
+
+        size = len(elements)
+        reordered = []
+        dir = -1 if backwards else 1
+        for i in range(size):
+            index = (i * dir + shiftStart * dir) % size
+            if index < 0:
+                index += size
+            reordered.append(elements[index])
+        return reordered
+
+    @staticmethod
+    def translationEDNToNWU(trl: Translation3d) -> Translation3d:
+        """Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
+        in EDN, this would be {0, -1, 0} in NWU.
+        """
+
+        return trl.rotateBy(EDN_TO_NWU)
+
+    @staticmethod
+    def rotationEDNToNWU(rot: Rotation3d) -> Rotation3d:
+        """Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0}
+        in NWU, this would be {0, 0, 1} in EDN.
+        """
+
+        return -EDN_TO_NWU + (rot + EDN_TO_NWU)
+
+    @staticmethod
+    def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:
+        """Returns a new 3d translation from this :class:`.Mat`. The opencv tvec is a vector with three
+        elements representing {x, y, z} in the EDN coordinate system.
+
+        :param tvecInput: The tvec to create a Translation3d from
+        """
+
+        return OpenCVHelp.translationEDNToNWU(Translation3d(tvecInput))
+
+    @staticmethod
+    def rVecToRotation(rvecInput: np.ndarray) -> Rotation3d:
+        """Returns a 3d rotation from this :class:`.Mat`. The opencv rvec Mat is a vector with three
+        elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm,
+        and axis = rvec / norm)
+
+        :param rvecInput: The rvec to create a Rotation3d from
+        """
+
+        return OpenCVHelp.rotationEDNToNWU(Rotation3d(rvecInput))
+
+    @staticmethod
+    def solvePNP_Square(
+        cameraMatrix: np.ndarray,
+        distCoeffs: np.ndarray,
+        modelTrls: list[Translation3d],
+        imagePoints: np.ndarray,
+    ) -> PnpResult | None:
+        """Finds the transformation(s) that map the camera's pose to the target's pose. The camera's pose
+        relative to the target is determined by the supplied 3d points of the target's model and their
+        associated 2d points imaged by the camera. The supplied model translations must be relative to
+        the target's pose.
+
+        For planar targets, there may be an alternate solution which is plausible given the 2d image
+        points. This has an associated "ambiguity" which describes the ratio of reprojection error
+        between the "best" and "alternate" solution.
+
+        This method is intended for use with individual AprilTags, and will not work unless 4 points
+        are provided.
+
+        :param cameraMatrix: The camera intrinsics matrix in standard opencv form
+        :param distCoeffs:   The camera distortion matrix in standard opencv form
+        :param modelTrls:    The translations of the object corners. These should have the object pose as
+                             their origin. These must come in a specific, pose-relative order (in NWU):
+
+                             - Point 0: [0, -squareLength / 2, squareLength / 2]
+                             - Point 1: [0, squareLength / 2, squareLength / 2]
+                             - Point 2: [0, squareLength / 2, -squareLength / 2]
+                             - Point 3: [0, -squareLength / 2, -squareLength / 2]
+        :param imagePoints:  The projection of these 3d object points into the 2d camera image. The order
+                             should match the given object point translations.
+
+        :returns: The resulting transformation that maps the camera pose to the target pose and the
+                  ambiguity if an alternate solution is available.
+        """
+        modelTrls = OpenCVHelp.reorderCircular(modelTrls, True, -1)
+        imagePoints = np.array(OpenCVHelp.reorderCircular(imagePoints, True, -1))
+        objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
+
+        alt: Transform3d | None = None
+        reprojectionError: cv.typing.MatLike | None = None
+        best: Transform3d = Transform3d()
+
+        for tries in range(2):
+            # calc rvecs/tvecs and associated reprojection error from image points
+            retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
+                objectMat,
+                imagePoints,
+                cameraMatrix,
+                distCoeffs,
+                flags=cv.SOLVEPNP_IPPE_SQUARE,
+            )
+
+            # convert to wpilib coordinates
+            best = Transform3d(
+                OpenCVHelp.tVecToTranslation(tvecs[0]),
+                OpenCVHelp.rVecToRotation(rvecs[0]),
+            )
+            if len(tvecs) > 1:
+                alt = Transform3d(
+                    OpenCVHelp.tVecToTranslation(tvecs[1]),
+                    OpenCVHelp.rVecToRotation(rvecs[1]),
+                )
+
+            # check if we got a NaN result
+            if reprojectionError is not None and not math.isnan(
+                reprojectionError[0, 0]
+            ):
+                break
+            else:
+                pt = imagePoints[0]
+                pt[0, 0] -= 0.001
+                pt[0, 1] -= 0.001
+                imagePoints[0] = pt
+
+        # solvePnP failed
+        if reprojectionError is None or math.isnan(reprojectionError[0, 0]):
+            logger.error("SolvePNP_Square failed!")
+            return None
+
+        if alt:
+            return PnpResult(
+                best=best,
+                bestReprojErr=reprojectionError[0, 0],
+                alt=alt,
+                altReprojErr=reprojectionError[1, 0],
+                ambiguity=reprojectionError[0, 0] / reprojectionError[1, 0],
+            )
+        else:
+            # We have no alternative so set it to best as well
+            return PnpResult(
+                best=best,
+                bestReprojErr=reprojectionError[0],
+                alt=best,
+                altReprojErr=reprojectionError[0],
+            )
+
+    @staticmethod
+    def solvePNP_SQPNP(
+        cameraMatrix: np.ndarray,
+        distCoeffs: np.ndarray,
+        modelTrls: list[Translation3d],
+        imagePoints: np.ndarray,
+    ) -> PnpResult | None:
+        """Finds the transformation that maps the camera's pose to the origin of the supplied object. An
+        "object" is simply a set of known 3d translations that correspond to the given 2d points. If,
+        for example, the object translations are given relative to close-right corner of the blue
+        alliance(the default origin), a camera-to-origin transformation is returned. If the
+        translations are relative to a target's pose, a camera-to-target transformation is returned.
+
+        There must be at least 3 points to use this method. This does not return an alternate
+        solution-- if you are intending to use solvePNP on a single AprilTag, see {@link
+        #solvePNP_SQUARE} instead.
+
+        :param cameraMatrix: The camera intrinsics matrix in standard opencv form
+        :param distCoeffs:   The camera distortion matrix in standard opencv form
+        :param objectTrls:   The translations of the object corners, relative to the field.
+        :param imagePoints:  The projection of these 3d object points into the 2d camera image. The order
+                             should match the given object point translations.
+
+        :returns: The resulting transformation that maps the camera pose to the target pose. If the 3d
+                  model points are supplied relative to the origin, this transformation brings the camera to
+                  the origin.
+        """
+
+        objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
+
+        retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
+            objectMat, imagePoints, cameraMatrix, distCoeffs, flags=cv.SOLVEPNP_SQPNP
+        )
+
+        error = reprojectionError[0, 0]
+        best = Transform3d(
+            OpenCVHelp.tVecToTranslation(tvecs[0]), OpenCVHelp.rVecToRotation(rvecs[0])
+        )
+
+        if math.isnan(error):
+            logger.error("SolvePNP_SQPNP failed!")
+            return None
+
+        # We have no alternative so set it to best as well
+        result = PnpResult(best=best, bestReprojErr=error, alt=best, altReprojErr=error)
+        return result

photonlibpy-2026.1.1rc3/photonlibpy/estimation/rotTrlTransform3d.py

@@ -0,0 +1,74 @@
+from typing import Self
+
+from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
+
+
+class RotTrlTransform3d:
+    """Represents a transformation that first rotates a pose around the origin, and then translates it."""
+
+    def __init__(
+        self, rot: Rotation3d = Rotation3d(), trl: Translation3d = Translation3d()
+    ):
+        """A rotation-translation transformation.
+
+        Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
+        transform as if the origin was transformed by these components instead.
+
+        :param rot: The rotation component
+        :param trl: The translation component
+        """
+        self.rot = rot
+        self.trl = trl
+
+    def inverse(self) -> Self:
+        """The inverse of this transformation. Applying the inverse will "undo" this transformation."""
+        invRot = -self.rot
+        invTrl = -(self.trl.rotateBy(invRot))
+        return type(self)(invRot, invTrl)
+
+    def getTransform(self) -> Transform3d:
+        """This transformation as a Transform3d (as if of the origin)"""
+        return Transform3d(self.trl, self.rot)
+
+    def getTranslation(self) -> Translation3d:
+        """The translation component of this transformation"""
+        return self.trl
+
+    def getRotation(self) -> Rotation3d:
+        """The rotation component of this transformation"""
+        return self.rot
+
+    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:
+        """The rotation-translation transformation that makes poses in the world consider this pose as the
+        new origin, or change the basis to this pose.
+
+        :param pose: The new origin
+        """
+        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()),
+        )