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

photonlibpy/simulation/photonCameraSim.py

@@ -26,6 +26,10 @@ from .visionTargetSim import VisionTargetSim
 
 
 class PhotonCameraSim:
+    """A handle for simulating :class:`.PhotonCamera` values. Processing simulated targets through this
+    class will change the associated PhotonCamera's results.
+    """
+
     kDefaultMinAreaPx: float = 100
 
     def __init__(
@@ -35,6 +39,21 @@ class PhotonCameraSim:
         minTargetAreaPercent: float | None = None,
         maxSightRange: meters | None = None,
     ):
+        """Constructs a handle for simulating :class:`.PhotonCamera` values. Processing simulated targets
+        through this class will change the associated PhotonCamera's results.
+
+        By default, this constructor's camera has a 90 deg FOV with no simulated lag if props!
+        By default, the minimum target area is 100 pixels and there is no maximum sight range unless both params are passed to override.
+
+
+        :param camera:               The camera to be simulated
+        :param prop:                 Properties of this camera such as FOV and FPS
+        :param minTargetAreaPercent: The minimum percentage(0 - 100) a detected target must take up of
+                                     the camera's image to be processed. Match this with your contour filtering settings in the
+                                     PhotonVision GUI.
+        :param maxSightRangeMeters:  Maximum distance at which the target is illuminated to your camera.
+                                     Note that minimum target area of the image is separate from this.
+        """
 
         self.minTargetAreaPercent: float = 0.0
         self.maxSightRange: float = 1.0e99
@@ -103,22 +122,39 @@ class PhotonCameraSim:
         return self.videoSimFrameRaw
 
     def canSeeTargetPose(self, camPose: Pose3d, target: VisionTargetSim) -> bool:
+        """Determines if this target's pose should be visible to the camera without considering its
+        projected image points. Does not account for image area.
+
+        :param camPose: Camera's 3d pose
+        :param target:  Vision target containing pose and shape
+
+        :returns: If this vision target can be seen before image projection.
+        """
+
         rel = CameraTargetRelation(camPose, target.getPose())
         return (
             (
+                # target translation is outside of camera's FOV
                 abs(rel.camToTargYaw.degrees())
                 < self.prop.getHorizFOV().degrees() / 2.0
                 and abs(rel.camToTargPitch.degrees())
                 < self.prop.getVertFOV().degrees() / 2.0
             )
             and (
+                # camera is behind planar target and it should be occluded
                 not target.getModel().getIsPlanar()
                 or abs(rel.targtoCamAngle.degrees()) < 90
             )
+            # target is too far
             and rel.camToTarg.translation().norm() <= self.maxSightRange
         )
 
     def canSeeCorner(self, points: np.ndarray) -> bool:
+        """Determines if all target points are inside the camera's image.
+
+        :param points: The target's 2d image points
+        """
+
         assert points.shape[1] == 1
         assert points.shape[2] == 2
         for pt in points:
@@ -130,51 +166,88 @@ class PhotonCameraSim:
                 or y < 0
                 or y > self.prop.getResHeight()
             ):
-                return False
+                return False  # point is outside of resolution
 
         return True
 
     def consumeNextEntryTime(self) -> float | None:
+        """Determine if this camera should process a new frame based on performance metrics and the time
+        since the last update. This returns an Optional which is either empty if no update should occur
+        or a Long of the timestamp in microseconds of when the frame which should be received by NT. If
+        a timestamp is returned, the last frame update time becomes that timestamp.
+
+        :returns: Optional long which is empty while blocked or the NT entry timestamp in microseconds if
+                  ready
+        """
+        # check if this camera is ready for another frame update
         now = int(wpilib.Timer.getFPGATimestamp() * 1e6)
         timestamp = 0
         iter = 0
+        # prepare next latest update
         while now >= self.nextNtEntryTime:
             timestamp = int(self.nextNtEntryTime)
             frameTime = int(self.prop.estSecUntilNextFrame() * 1e6)
             self.nextNtEntryTime += frameTime
 
+            # if frame time is very small, avoid blocking
             iter += 1
             if iter > 50:
                 timestamp = now
                 self.nextNtEntryTime = now + frameTime
                 break
 
+        # return the timestamp of the latest update
         if timestamp != 0:
             return timestamp
 
+        # or this camera isn't ready to process yet
         return None
 
     def setMinTargetAreaPercent(self, areaPercent: float) -> None:
+        """The minimum percentage(0 - 100) a detected target must take up of the camera's image to be
+        processed.
+        """
         self.minTargetAreaPercent = areaPercent
 
     def setMinTargetAreaPixels(self, areaPx: float) -> None:
+        """The minimum number of pixels a detected target must take up in the camera's image to be
+        processed.
+        """
         self.minTargetAreaPercent = areaPx / self.prop.getResArea() * 100.0
 
     def setMaxSightRange(self, range: meters) -> None:
+        """Maximum distance at which the target is illuminated to your camera. Note that minimum target
+        area of the image is separate from this.
+        """
         self.maxSightRange = range
 
     def enableRawStream(self, enabled: bool) -> None:
+        """Sets whether the raw video stream simulation is enabled.
+
+        Note: This may increase loop times.
+        """
         self.videoSimRawEnabled = enabled
         raise Exception("Raw stream not implemented")
 
     def enableDrawWireframe(self, enabled: bool) -> None:
+        """Sets whether a wireframe of the field is drawn to the raw video stream.
+
+        Note: This will dramatically increase loop times.
+        """
         self.videoSimWireframeEnabled = enabled
         raise Exception("Wireframe not implemented")
 
     def setWireframeResolution(self, resolution: float) -> None:
+        """Sets the resolution of the drawn wireframe if enabled. Drawn line segments will be subdivided
+        into smaller segments based on a threshold set by the resolution.
+
+        :param resolution: Resolution as a fraction(0 - 1) of the video frame's diagonal length in
+                           pixels
+        """
         self.videoSimWireframeResolution = resolution
 
     def enableProcessedStream(self, enabled: bool) -> None:
+        """Sets whether the processed video stream simulation is enabled."""
         self.videoSimProcEnabled = enabled
         raise Exception("Processed stream not implemented")
 
@@ -187,25 +260,32 @@ class PhotonCameraSim:
 
         targets.sort(key=distance, reverse=True)
 
+        # all targets visible before noise
         visibleTgts: list[typing.Tuple[VisionTargetSim, np.ndarray]] = []
+        # all targets actually detected by camera (after noise)
         detectableTgts: list[PhotonTrackedTarget] = []
 
+        # basis change from world coordinates to camera coordinates
         camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
 
         for tgt in targets:
+            # pose isn't visible, skip to next
             if not self.canSeeTargetPose(cameraPose, tgt):
                 continue
 
+            # find target's 3d corner points
             fieldCorners = tgt.getFieldVertices()
             isSpherical = tgt.getModel().getIsSpherical()
-            if isSpherical:
+            if isSpherical:  # target is spherical
                 model = tgt.getModel()
+                # orient the model to the camera (like a sprite/decal) so it appears similar regardless of view
                 fieldCorners = model.getFieldVertices(
                     TargetModel.getOrientedPose(
                         tgt.getPose().translation(), cameraPose.translation()
                     )
                 )
 
+            # project 3d target points into 2d image points
             imagePoints = OpenCVHelp.projectPoints(
                 self.prop.getIntrinsics(),
                 self.prop.getDistCoeffs(),
@@ -213,9 +293,11 @@ class PhotonCameraSim:
                 fieldCorners,
             )
 
+            # spherical targets need a rotated rectangle of their midpoints for visualization
             if isSpherical:
                 center = OpenCVHelp.avgPoint(imagePoints)
                 l: int = 0
+                # reference point (left side midpoint)
                 for i in range(4):
                     if imagePoints[i, 0, 0] < imagePoints[l, 0, 0].x:
                         l = i
@@ -239,6 +321,7 @@ class PhotonCameraSim:
                 for i in range(4):
                     if i != t and i != l and i != b:
                         r = i
+                # create RotatedRect from midpoints
                 rect = cv.RotatedRect(
                     (center[0, 0], center[0, 1]),
                     (
@@ -247,16 +330,23 @@ class PhotonCameraSim:
                     ),
                     -angles[r],
                 )
+                # set target corners to rect corners
                 imagePoints = np.array([[p[0], p[1], p[2]] for p in rect.points()])
 
+            # save visible targets for raw video stream simulation
             visibleTgts.append((tgt, imagePoints))
+            # estimate pixel noise
             noisyTargetCorners = self.prop.estPixelNoise(imagePoints)
+            # find the minimum area rectangle of target corners
             minAreaRect = OpenCVHelp.getMinAreaRect(noisyTargetCorners)
             minAreaRectPts = minAreaRect.points()
+            # find the (naive) 2d yaw/pitch
             centerPt = minAreaRect.center
             centerRot = self.prop.getPixelRot(centerPt)
+            # find contour area
             areaPercent = self.prop.getContourAreaPercent(noisyTargetCorners)
 
+            # projected target can't be detected, skip to next
             if (
                 not self.canSeeCorner(noisyTargetCorners)
                 or not areaPercent >= self.minTargetAreaPercent
@@ -265,6 +355,7 @@ class PhotonCameraSim:
 
             pnpSim: PnpResult | None = None
             if tgt.fiducialId >= 0 and len(tgt.getFieldVertices()) == 4:
+                # single AprilTag solvePNP
                 pnpSim = OpenCVHelp.solvePNP_Square(
                     self.prop.getIntrinsics(),
                     self.prop.getDistCoeffs(),
@@ -295,6 +386,7 @@ class PhotonCameraSim:
 
         # Video streams disabled for now
         if self.videoSimRawEnabled:
+            # TODO Video streams disabled for now port and uncomment when implemented
             # VideoSimUtil::UpdateVideoProp(videoSimRaw, prop);
             # cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()};
             # cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1);
@@ -312,6 +404,7 @@ class PhotonCameraSim:
 
         if len(visibleLayoutTags) > 1:
             usedIds = [tag.ID for tag in visibleLayoutTags]
+            # sort target order sorts in ascending order by default
             usedIds.sort()
             pnpResult = VisionEstimation.estimateCamPosePNP(
                 self.prop.getIntrinsics(),
@@ -323,10 +416,16 @@ class PhotonCameraSim:
             if pnpResult is not None:
                 multiTagResults = MultiTargetPNPResult(pnpResult, usedIds)
 
+        # put this simulated data to NT
         self.heartbeatCounter += 1
+        now_micros = wpilib.Timer.getFPGATimestamp() * 1e6
         return PhotonPipelineResult(
             metadata=PhotonPipelineMetadata(
-                self.heartbeatCounter, int(latency * 1e6), 1000000
+                self.heartbeatCounter,
+                int(now_micros - latency * 1e6),
+                int(now_micros),
+                # Pretend like we heard a pong recently
+                int(np.random.uniform(950, 1050)),
             ),
             targets=detectableTgts,
             multitagResult=multiTagResults,
@@ -335,6 +434,13 @@ class PhotonCameraSim:
     def submitProcessedFrame(
         self, result: PhotonPipelineResult, receiveTimestamp: float | None
     ):
+        """Simulate one processed frame of vision data, putting one result to NT. Image capture timestamp
+        overrides :meth:`.PhotonPipelineResult.getTimestampSeconds` for more
+        precise latency simulation.
+
+        :param result:           The pipeline result to submit
+        :param receiveTimestamp: The (sim) timestamp when this result was read by NT in microseconds. If not passed image capture time is assumed be (current time - latency)
+        """
         if receiveTimestamp is None:
             receiveTimestamp = wpilib.Timer.getFPGATimestamp() * 1e6
         receiveTimestamp = int(receiveTimestamp)

photonlibpy/simulation/simCameraProperties.py

@@ -11,7 +11,22 @@ from ..estimation import RotTrlTransform3d
 
 
 class SimCameraProperties:
+    """Calibration and performance values for this camera.
+
+    The resolution will affect the accuracy of projected(3d to 2d) target corners and similarly
+    the severity of image noise on estimation(2d to 3d).
+
+    The camera intrinsics and distortion coefficients describe the results of calibration, and how
+    to map between 3d field points and 2d image points.
+
+    The performance values (framerate/exposure time, latency) determine how often results should
+    be updated and with how much latency in simulation. High exposure time causes motion blur which
+    can inhibit target detection while moving. Note that latency estimation does not account for
+    network latency and the latency reported will always be perfect.
+    """
+
     def __init__(self):
+        """Default constructor which is the same as {@link #PERFECT_90DEG}"""
         self.resWidth: int = -1
         self.resHeight: int = -1
         self.camIntrinsics: np.ndarray = np.zeros((3, 3))  # [3,3]
@@ -38,14 +53,18 @@ class SimCameraProperties:
         fovWidth = Rotation2d(math.atan((diagRatio * (width / resDiag)) * 2))
         fovHeight = Rotation2d(math.atan(diagRatio * (height / resDiag)) * 2)
 
+        # assume no distortion
         newDistCoeffs = np.zeros((8, 1))
 
+        # assume centered principal point (pixels)
         cx = width / 2.0 - 0.5
         cy = height / 2.0 - 0.5
 
+        # use given fov to determine focal point (pixels)
         fx = cx / math.tan(fovWidth.radians() / 2.0)
         fy = cy / math.tan(fovHeight.radians() / 2.0)
 
+        # create camera intrinsics matrix
         newCamIntrinsics = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]])
 
         self.setCalibrationFromIntrinsics(
@@ -65,6 +84,7 @@ class SimCameraProperties:
         self.camIntrinsics = newCamIntrinsics
         self.distCoeffs = newDistCoeffs
 
+        # left, right, up, and down view planes
         p = [
             Translation3d(
                 1.0,
@@ -110,16 +130,33 @@ class SimCameraProperties:
         self.errorStdDevPx = newErrorStdDevPx
 
     def setFPS(self, fps: hertz):
+        """
+        :param fps: The average frames per second the camera should process at. :strong:`Exposure time limits
+                    FPS if set!`
+        """
+
         self.frameSpeed = max(1.0 / fps, self.exposureTime)
 
     def setExposureTime(self, newExposureTime: seconds):
+        """
+        :param newExposureTime: The amount of time the "shutter" is open for one frame. Affects motion
+                               blur. **Frame speed(from FPS) is limited to this!**
+        """
+
         self.exposureTime = newExposureTime
         self.frameSpeed = max(self.frameSpeed, self.exposureTime)
 
     def setAvgLatency(self, newAvgLatency: seconds):
+        """
+        :param newAvgLatency: The average latency (from image capture to data published) in milliseconds
+                             a frame should have
+        """
         self.vgLatency = newAvgLatency
 
     def setLatencyStdDev(self, newLatencyStdDev: seconds):
+        """
+        :param latencyStdDevMs: The standard deviation in milliseconds of the latency
+        """
         self.latencyStdDev = newLatencyStdDev
 
     def getResWidth(self) -> int:
@@ -156,21 +193,43 @@ class SimCameraProperties:
         return self.latencyStdDev
 
     def getContourAreaPercent(self, points: np.ndarray) -> float:
+        """The percentage(0 - 100) of this camera's resolution the contour takes up in pixels of the
+        image.
+
+        :param points: Points of the contour
+        """
+
         return cv.contourArea(cv.convexHull(points)) / self.getResArea() * 100.0
 
     def getPixelYaw(self, pixelX: float) -> Rotation2d:
+        """The yaw from the principal point of this camera to the pixel x value. Positive values left."""
         fx = self.camIntrinsics[0, 0]
+        # account for principal point not being centered
         cx = self.camIntrinsics[0, 2]
         xOffset = cx - pixelX
         return Rotation2d(fx, xOffset)
 
     def getPixelPitch(self, pixelY: float) -> Rotation2d:
+        """The pitch from the principal point of this camera to the pixel y value. Pitch is positive down.
+
+        Note that this angle is naively computed and may be incorrect. See {@link
+        #getCorrectedPixelRot(Point)}.
+        """
+
         fy = self.camIntrinsics[1, 1]
+        # account for principal point not being centered
         cy = self.camIntrinsics[1, 2]
         yOffset = cy - pixelY
         return Rotation2d(fy, -yOffset)
 
     def getPixelRot(self, point: cv.typing.Point2f) -> Rotation3d:
+        """Finds the yaw and pitch to the given image point. Yaw is positive left, and pitch is positive
+        down.
+
+        Note that pitch is naively computed and may be incorrect. See {@link
+        #getCorrectedPixelRot(Point)}.
+        """
+
         return Rotation3d(
             0.0,
             self.getPixelPitch(point[1]).radians(),
@@ -178,6 +237,27 @@ class SimCameraProperties:
         )
 
     def getCorrectedPixelRot(self, point: cv.typing.Point2f) -> Rotation3d:
+        """Gives the yaw and pitch of the line intersecting the camera lens and the given pixel
+        coordinates on the sensor. Yaw is positive left, and pitch positive down.
+
+        The pitch traditionally calculated from pixel offsets do not correctly account for non-zero
+        values of yaw because of perspective distortion (not to be confused with lens distortion)-- for
+        example, the pitch angle is naively calculated as:
+
+        <pre>pitch = arctan(pixel y offset / focal length y)</pre>
+
+        However, using focal length as a side of the associated right triangle is not correct when the
+        pixel x value is not 0, because the distance from this pixel (projected on the x-axis) to the
+        camera lens increases. Projecting a line back out of the camera with these naive angles will
+        not intersect the 3d point that was originally projected into this 2d pixel. Instead, this
+        length should be:
+
+        <pre>focal length y ⟶ (focal length y / cos(arctan(pixel x offset / focal length x)))</pre>
+
+        :returns: Rotation3d with yaw and pitch of the line projected out of the camera from the given
+                  pixel (roll is zero).
+        """
+
         fx = self.camIntrinsics[0, 0]
         cx = self.camIntrinsics[0, 2]
         xOffset = cx - point[0]
@@ -191,11 +271,13 @@ class SimCameraProperties:
         return Rotation3d(0.0, pitch.radians(), yaw.radians())
 
     def getHorizFOV(self) -> Rotation2d:
+        # sum of FOV left and right principal point
         left = self.getPixelYaw(0)
         right = self.getPixelYaw(self.resWidth)
         return left - right
 
     def getVertFOV(self) -> Rotation2d:
+        # sum of FOV above and below principal point
         above = self.getPixelPitch(0)
         below = self.getPixelPitch(self.resHeight)
         return below - above
@@ -208,9 +290,34 @@ class SimCameraProperties:
     def getVisibleLine(
         self, camRt: RotTrlTransform3d, a: Translation3d, b: Translation3d
     ) -> typing.Tuple[float | None, float | None]:
+        """Determines where the line segment defined by the two given translations intersects the camera's
+        frustum/field-of-vision, if at all.
+
+        The line is parametrized so any of its points <code>p = t * (b - a) + a</code>. This method
+        returns these values of t, minimum first, defining the region of the line segment which is
+        visible in the frustum. If both ends of the line segment are visible, this simply returns {0,
+        1}. If, for example, point b is visible while a is not, and half of the line segment is inside
+        the camera frustum, {0.5, 1} would be returned.
+
+        :param camRt: The change in basis from world coordinates to camera coordinates. See {@link
+                      RotTrlTransform3d#makeRelativeTo(Pose3d)}.
+        :param a:     The initial translation of the line
+        :param b:     The final translation of the line
+
+        :returns: A Pair of Doubles. The values may be null:
+
+                  - {Double, Double} : Two parametrized values(t), minimum first, representing which
+                  segment of the line is visible in the camera frustum.
+                  - {Double, null} : One value(t) representing a single intersection point. For example,
+                  the line only intersects the intersection of two adjacent viewplanes.
+                  - {null, null} : No values. The line segment is not visible in the camera frustum.
+        """
+
+        # translations relative to the camera
         relA = camRt.applyTranslation(a)
         relB = camRt.applyTranslation(b)
 
+        # check if both ends are behind camera
         if relA.X() <= 0.0 and relB.X() <= 0.0:
             return (None, None)
 
@@ -221,6 +328,7 @@ class SimCameraProperties:
         aVisible = True
         bVisible = True
 
+        # check if the ends of the line segment are visible
         for normal in self.viewplanes:
             aVisibility = av.dot(normal)
             if aVisibility < 0:
@@ -229,38 +337,54 @@ class SimCameraProperties:
             bVisibility = bv.dot(normal)
             if bVisibility < 0:
                 bVisible = False
+            # both ends are outside at least one of the same viewplane
             if aVisibility <= 0 and bVisibility <= 0:
                 return (None, None)
 
+        # both ends are inside frustum
         if aVisible and bVisible:
             return (0.0, 1.0)
 
+        # parametrized (t=0 at a, t=1 at b) intersections with viewplanes
         intersections = [float("nan"), float("nan"), float("nan"), float("nan")]
 
         # Optionally 3x1 vector
         ipts: typing.List[np.ndarray | None] = [None, None, None, None]
 
+        # find intersections
         for i, normal in enumerate(self.viewplanes):
+
+            # // we want to know the value of t when the line intercepts this plane
+            # // parametrized: v = t * ab + a, where v lies on the plane
+            # // we can find the projection of a onto the plane normal
+            # // a_projn = normal.times(av.dot(normal) / normal.dot(normal));
             a_projn = (av.dot(normal) / normal.dot(normal)) * normal
 
+            # // this projection lets us determine the scalar multiple t of ab where
+            # // (t * ab + a) is a vector which lies on the plane
             if abs(abv.dot(normal)) < 1.0e-5:
                 continue
             intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn))
 
+            # // vector a to the viewplane
             apv = intersections[i] * abv
+            # av + apv = intersection point
             intersectpt = av + apv
             ipts[i] = intersectpt
 
+            # // discard intersections outside the camera frustum
             for j in range(1, len(self.viewplanes)):
                 if j == 0:
                     continue
                 oi = (i + j) % len(self.viewplanes)
                 onormal = self.viewplanes[oi]
+                # if the dot of the intersection point with any plane normal is negative, it is outside
                 if intersectpt.dot(onormal) < 0:
                     intersections[i] = float("nan")
                     ipts[i] = None
                     break
 
+            # // discard duplicate intersections
             if ipts[i] is None:
                 continue
 
@@ -275,6 +399,7 @@ class SimCameraProperties:
                     ipts[i] = None
                     break
 
+        # determine visible segment (minimum and maximum t)
         inter1 = float("nan")
         inter2 = float("nan")
         for inter in intersections:
@@ -284,6 +409,7 @@ class SimCameraProperties:
                 else:
                     inter2 = inter
 
+        # // two viewplane intersections
         if not math.isnan(inter2):
             max_ = max(inter1, inter2)
             min_ = min(inter1, inter2)
@@ -292,16 +418,19 @@ class SimCameraProperties:
             if bVisible:
                 max_ = 1
             return (min_, max_)
+        # // one viewplane intersection
         elif not math.isnan(inter1):
             if aVisible:
                 return (0, inter1)
             if bVisible:
                 return (inter1, 1)
             return (inter1, None)
+        # no intersections
         else:
             return (None, None)
 
     def estPixelNoise(self, points: np.ndarray) -> np.ndarray:
+        """Returns these points after applying this camera's estimated noise."""
         assert points.shape[1] == 1, points.shape
         assert points.shape[2] == 2, points.shape
         if self.avgErrorPx == 0 and self.errorStdDevPx == 0:
@@ -309,6 +438,7 @@ class SimCameraProperties:
 
         noisyPts: list[list] = []
         for p in points:
+            # // error pixels in random direction
             error = np.random.normal(self.avgErrorPx, self.errorStdDevPx, 1)[0]
             errorAngle = np.random.uniform(-math.pi, math.pi)
             noisyPts.append(
@@ -324,16 +454,25 @@ class SimCameraProperties:
         return retval
 
     def estLatency(self) -> seconds:
+        """
+        :returns: Noisy estimation of a frame's processing latency
+        """
+
         return max(
             float(np.random.normal(self.avgLatency, self.latencyStdDev, 1)[0]),
             0.0,
         )
 
     def estSecUntilNextFrame(self) -> seconds:
+        """
+        :returns: Estimate how long until the next frame should be processed in milliseconds
+        """
+        # // exceptional processing latency blocks the next frame
         return self.frameSpeed + max(0.0, self.estLatency() - self.frameSpeed)
 
     @classmethod
     def PERFECT_90DEG(cls) -> typing.Self:
+        """960x720 resolution, 90 degree FOV, "perfect" lagless camera"""
         return cls()
 
     @classmethod