photonlibpy 2025.0.0b3__py3-none-any.whl → 2025.0.0b4__py3-none-any.whl

photonlibpy/estimation/openCVHelp.py

@@ -27,6 +27,12 @@ class OpenCVHelp:
 
     @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)
@@ -38,6 +44,13 @@ class OpenCVHelp:
 
     @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()
@@ -88,6 +101,25 @@ class OpenCVHelp:
     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
@@ -100,18 +132,39 @@ class OpenCVHelp:
 
     @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
@@ -121,6 +174,33 @@ class OpenCVHelp:
         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))
@@ -130,6 +210,7 @@ class OpenCVHelp:
         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,
@@ -138,6 +219,7 @@ class OpenCVHelp:
                 flags=cv.SOLVEPNP_IPPE_SQUARE,
             )
 
+            # convert to wpilib coordinates
             best = Transform3d(
                 OpenCVHelp.tVecToTranslation(tvecs[0]),
                 OpenCVHelp.rVecToRotation(rvecs[0]),
@@ -148,6 +230,7 @@ class OpenCVHelp:
                     OpenCVHelp.rVecToRotation(rvecs[1]),
                 )
 
+            # check if we got a NaN result
             if reprojectionError is not None and not math.isnan(
                 reprojectionError[0, 0]
             ):
@@ -158,6 +241,7 @@ class OpenCVHelp:
                 pt[0, 1] -= 0.001
                 imagePoints[0] = pt
 
+        # solvePnP failed
         if reprojectionError is None or math.isnan(reprojectionError[0, 0]):
             print("SolvePNP_Square failed!")
             return None
@@ -186,6 +270,27 @@ class OpenCVHelp:
         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(

photonlibpy/estimation/rotTrlTransform3d.py

@@ -4,24 +4,38 @@ 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:
@@ -44,6 +58,11 @@ class RotTrlTransform3d:
 
     @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

photonlibpy/estimation/targetModel.py

@@ -8,14 +8,27 @@ from . import RotTrlTransform3d
 
 
 class TargetModel:
+    """Describes the 3d model of a target."""
 
     def __init__(self):
+        """Default constructor for initialising internal class members. DO NOT USE THIS!!! USE THE createPlanar,
+        createCuboid, createSpheroid or create Arbitrary
+        """
         self.vertices: List[Translation3d] = []
         self.isPlanar = False
         self.isSpherical = False
 
     @classmethod
     def createPlanar(cls, width: meters, height: meters) -> Self:
+        """Creates a rectangular, planar target model given the width and height. The model has four
+        vertices:
+
+        - Point 0: [0, -width/2, -height/2]
+        - Point 1: [0, width/2, -height/2]
+        - Point 2: [0, width/2, height/2]
+        - Point 3: [0, -width/2, height/2]
+        """
+
         tm = cls()
 
         tm.isPlanar = True
@@ -30,6 +43,18 @@ class TargetModel:
 
     @classmethod
     def createCuboid(cls, length: meters, width: meters, height: meters) -> Self:
+        """Creates a cuboid target model given the length, width, height. The model has eight vertices:
+
+        - Point 0: [length/2, -width/2, -height/2]
+        - Point 1: [length/2, width/2, -height/2]
+        - Point 2: [length/2, width/2, height/2]
+        - Point 3: [length/2, -width/2, height/2]
+        - Point 4: [-length/2, -width/2, height/2]
+        - Point 5: [-length/2, width/2, height/2]
+        - Point 6: [-length/2, width/2, -height/2]
+        - Point 7: [-length/2, -width/2, -height/2]
+        """
+
         tm = cls()
         verts = [
             Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
@@ -48,6 +73,20 @@ class TargetModel:
 
     @classmethod
     def createSpheroid(cls, diameter: meters) -> Self:
+        """Creates a spherical target model which has similar dimensions regardless of its rotation. This
+        model has four vertices:
+
+        - Point 0: [0, -radius, 0]
+        - Point 1: [0, 0, -radius]
+        - Point 2: [0, radius, 0]
+        - Point 3: [0, 0, radius]
+
+        *Q: Why these vertices?* A: This target should be oriented to the camera every frame, much
+        like a sprite/decal, and these vertices represent the ellipse vertices (maxima). These vertices
+        are used for drawing the image of this sphere, but do not match the corners that will be
+        published by photonvision.
+        """
+
         tm = cls()
 
         tm.isPlanar = False
@@ -63,6 +102,14 @@ class TargetModel:
 
     @classmethod
     def createArbitrary(cls, verts: List[Translation3d]) -> Self:
+        """Creates a target model from arbitrary 3d vertices. Automatically determines if the given
+        vertices are planar(x == 0). More than 2 vertices must be given. If this is a planar model, the
+        vertices should define a non-intersecting contour.
+
+        :param vertices: Translations representing the vertices of this target model relative to its
+                         pose.
+        """
+
         tm = cls()
         tm._common_construction(verts)
 
@@ -83,6 +130,12 @@ class TargetModel:
         self.vertices = verts
 
     def getFieldVertices(self, targetPose: Pose3d) -> List[Translation3d]:
+        """This target's vertices offset from its field pose.
+
+        Note: If this target is spherical, use {@link #getOrientedPose(Translation3d,
+        Translation3d)} with this method.
+        """
+
         basisChange = RotTrlTransform3d(targetPose.rotation(), targetPose.translation())
 
         retVal = []
@@ -94,6 +147,16 @@ class TargetModel:
 
     @classmethod
     def getOrientedPose(cls, tgtTrl: Translation3d, cameraTrl: Translation3d):
+        """Returns a Pose3d with the given target translation oriented (with its relative x-axis aligned)
+        to the camera translation. This is used for spherical targets which should not have their
+        projection change regardless of their own rotation.
+
+        :param tgtTrl:    This target's translation
+        :param cameraTrl: Camera's translation
+
+        :returns: This target's pose oriented to the camera
+        """
+
         relCam = cameraTrl - tgtTrl
         orientToCam = Rotation3d(
             0.0,

photonlibpy/estimation/visionEstimation.py

@@ -11,6 +11,7 @@ class VisionEstimation:
     def getVisibleLayoutTags(
         visTags: list[PhotonTrackedTarget], layout: AprilTagFieldLayout
     ) -> list[AprilTag]:
+        """Get the visible :class:`.AprilTag`s which are in the tag layout using the visible tag IDs."""
         retVal: list[AprilTag] = []
         for tag in visTags:
             id = tag.getFiducialId()
@@ -30,12 +31,31 @@ class VisionEstimation:
         layout: AprilTagFieldLayout,
         tagModel: TargetModel,
     ) -> PnpResult | None:
+        """Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the
+        field-to-camera transformation. If only one tag is visible, the result may have an alternate
+        solution.
+
+        **Note:** The returned transformation is from the field origin to the camera pose!
+
+        With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}
+
+        With multiple tags: {@link OpenCVHelp#solvePNP_SQPNP}
+
+        :param cameraMatrix: The camera intrinsics matrix in standard opencv form
+        :param distCoeffs:   The camera distortion matrix in standard opencv form
+        :param visTags:      The visible tags reported by PV. Non-tag targets are automatically excluded.
+        :param tagLayout:    The known tag layout on the field
+
+        :returns: The transformation that maps the field origin to the camera pose. Ensure the {@link
+                  PnpResult} are present before utilizing them.
+        """
         if len(visTags) == 0:
             return None
 
         corners: list[TargetCorner] = []
         knownTags: list[AprilTag] = []
 
+        # ensure these are AprilTags in our layout
         for tgt in visTags:
             id = tgt.getFiducialId()
             maybePose = layout.getTagPose(id)
@@ -53,6 +73,7 @@ class VisionEstimation:
 
         points = OpenCVHelp.cornersToPoints(corners)
 
+        # single-tag pnp
         if len(knownTags) == 1:
             camToTag = OpenCVHelp.solvePNP_Square(
                 cameraMatrix, distCoeffs, tagModel.getVertices(), points
@@ -74,6 +95,7 @@ class VisionEstimation:
                 altReprojErr=camToTag.altReprojErr,
             )
             return result
+        # multi-tag pnp
         else:
             objectTrls: list[Translation3d] = []
             for tag in knownTags:

photonlibpy/networktables/NTTopicSet.py

@@ -9,6 +9,13 @@ PhotonPipelineResult_TYPE_STRING = (
 
 
 class NTTopicSet:
+    """This class is a wrapper around all per-pipeline NT topics that PhotonVision should be publishing
+    It's split here so the sim and real-camera implementations can share a common implementation of
+    the naming and registration of the NT content.
+
+    However, we do expect that the actual logic which fills out values in the entries will be
+    different for sim vs. real camera
+    """
 
     def __init__(self, tableName: str, cameraName: str) -> None:
         instance = nt.NetworkTableInstance.getDefault()

photonlibpy/photonCamera.py

@@ -48,6 +48,10 @@ def setVersionCheckEnabled(enabled: bool):
 
 class PhotonCamera:
     def __init__(self, cameraName: str):
+        """Constructs a PhotonCamera from the name of the camera.
+
+        :param cameraName: The nickname of the camera (found in the PhotonVision UI).
+        """
         instance = ntcore.NetworkTableInstance.getDefault()
         self._name = cameraName
         self._tableName = "photonvision"
@@ -132,6 +136,14 @@ class PhotonCamera:
         return ret
 
     def getLatestResult(self) -> PhotonPipelineResult:
+        """Returns the latest pipeline result. This is simply the most recent result Received via NT.
+        Calling this multiple times will always return the most recent result.
+
+        Replaced by :meth:`.getAllUnreadResults` over getLatestResult, as this function can miss
+        results, or provide duplicate ones!
+        TODO implement the thing that will take this ones place...
+        """
+
         self._versionCheck()
 
         now = RobotController.getFPGATime()
@@ -149,34 +161,85 @@ class PhotonCamera:
             return retVal
 
     def getDriverMode(self) -> bool:
+        """Returns whether the camera is in driver mode.
+
+        :returns: Whether the camera is in driver mode.
+        """
+
         return self._driverModeSubscriber.get()
 
     def setDriverMode(self, driverMode: bool) -> None:
+        """Toggles driver mode.
+
+        :param driverMode: Whether to set driver mode.
+        """
+
         self._driverModePublisher.set(driverMode)
 
     def takeInputSnapshot(self) -> None:
+        """Request the camera to save a new image file from the input camera stream with overlays. Images
+        take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk
+        space and eventually cause the system to stop working. Clear out images in
+        /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
+        """
+
         self._inputSaveImgEntry.set(self._inputSaveImgEntry.get() + 1)
 
     def takeOutputSnapshot(self) -> None:
+        """Request the camera to save a new image file from the output stream with overlays. Images take
+        up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space
+        and eventually cause the system to stop working. Clear out images in
+        /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
+        """
         self._outputSaveImgEntry.set(self._outputSaveImgEntry.get() + 1)
 
     def getPipelineIndex(self) -> int:
+        """Returns the active pipeline index.
+
+        :returns: The active pipeline index.
+        """
+
         return self._pipelineIndexState.get(0)
 
     def setPipelineIndex(self, index: int) -> None:
+        """Allows the user to select the active pipeline index.
+
+        :param index: The active pipeline index.
+        """
         self._pipelineIndexRequest.set(index)
 
     def getLEDMode(self) -> VisionLEDMode:
+        """Returns the current LED mode.
+
+        :returns: The current LED mode.
+        """
+
         mode = self._ledModeState.get()
         return VisionLEDMode(mode)
 
     def setLEDMode(self, led: VisionLEDMode) -> None:
+        """Sets the LED mode.
+
+        :param led: The mode to set to.
+        """
+
         self._ledModeRequest.set(led.value)
 
     def getName(self) -> str:
+        """Returns the name of the camera. This will return the same value that was given to the
+        constructor as cameraName.
+
+        :returns: The name of the camera.
+        """
         return self._name
 
     def isConnected(self) -> bool:
+        """Returns whether the camera is connected and actively returning new data. Connection status is
+        debounced.
+
+        :returns: True if the camera is actively sending frame data, false otherwise.
+        """
+
         curHeartbeat = self._heartbeatEntry.get()
         now = Timer.getFPGATimestamp()
 
@@ -197,6 +260,8 @@ class PhotonCamera:
 
         _lastVersionTimeCheck = Timer.getFPGATimestamp()
 
+        # Heartbeat entry is assumed to always be present. If it's not present, we
+        # assume that a camera with that name was never connected in the first place.
         if not self._heartbeatEntry.exists():
             cameraNames = (
                 self._cameraTable.getInstance().getTable(self._tableName).getSubTables()
@@ -222,6 +287,7 @@ class PhotonCamera:
                     True,
                 )
 
+        # Check for connection status. Warn if disconnected.
         elif not self.isConnected():
             wpilib.reportWarning(
                 f"PhotonVision coprocessor at path {self._path} is not sending new data.",
@@ -229,8 +295,9 @@ class PhotonCamera:
             )
 
         versionString = self.versionEntry.get(defaultValue="")
-        localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
 
+        # Check mdef UUID
+        localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
         remoteUUID = str(self._rawBytesEntry.getTopic().getProperty("message_uuid"))
 
         if not remoteUUID: