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

photonlibpy/simulation/simCameraProperties.py

@@ -11,7 +11,22 @@ from ..estimation import RotTrlTransform3d
 
 
 class SimCameraProperties:
-    def __init__(self, path: str | None = None, width: int = 0, height: int = 0):
+    """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]
@@ -24,63 +39,52 @@ class SimCameraProperties:
         self.latencyStdDev: seconds = 0.0
         self.viewplanes: list[np.ndarray] = []  # [3,1]
 
-        if path is None:
-            self.setCalibration(960, 720, fovDiag=Rotation2d(math.radians(90.0)))
-        else:
-            raise Exception("not yet implemented")
-
-    def setCalibration(
-        self,
-        width: int,
-        height: int,
-        *,
-        fovDiag: Rotation2d | None = None,
-        newCamIntrinsics: np.ndarray | None = None,
-        newDistCoeffs: np.ndarray | None = None,
-    ):
-        # Should be an inverted XOR on the args to differentiate between the signatures
-
-        has_fov_args = fovDiag is not None
-        has_matrix_args = newCamIntrinsics is not None and newDistCoeffs is not None
+        self.setCalibrationFromFOV(960, 720, fovDiag=Rotation2d(math.radians(90.0)))
 
-        if (has_fov_args and has_matrix_args) or (
-            not has_matrix_args and not has_fov_args
-        ):
-            raise Exception("not a correct function sig")
+    def setCalibrationFromFOV(
+        self, width: int, height: int, fovDiag: Rotation2d
+    ) -> None:
+        if fovDiag.degrees() < 1.0 or fovDiag.degrees() > 179.0:
+            fovDiag = Rotation2d.fromDegrees(max(min(fovDiag.degrees(), 179.0), 1.0))
+            logging.error("Requested invalid FOV! Clamping between (1, 179) degrees...")
 
-        if has_fov_args:
-            if fovDiag.degrees() < 1.0 or fovDiag.degrees() > 179.0:
-                fovDiag = Rotation2d.fromDegrees(
-                    max(min(fovDiag.degrees(), 179.0), 1.0)
-                )
-                logging.error(
-                    "Requested invalid FOV! Clamping between (1, 179) degrees..."
-                )
+        resDiag = math.sqrt(width * width + height * height)
+        diagRatio = math.tan(fovDiag.radians() / 2.0)
+        fovWidth = Rotation2d(math.atan((diagRatio * (width / resDiag)) * 2))
+        fovHeight = Rotation2d(math.atan(diagRatio * (height / resDiag)) * 2)
 
-            resDiag = math.sqrt(width * width + height * height)
-            diagRatio = math.tan(fovDiag.radians() / 2.0)
-            fovWidth = Rotation2d(math.atan((diagRatio * (width / resDiag)) * 2))
-            fovHeight = Rotation2d(math.atan(diagRatio * (height / resDiag)) * 2)
+        # assume no distortion
+        newDistCoeffs = np.zeros((8, 1))
 
-            newDistCoeffs = np.zeros((8, 1))
+        # assume centered principal point (pixels)
+        cx = width / 2.0 - 0.5
+        cy = height / 2.0 - 0.5
 
-            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)
 
-            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]])
 
-            newCamIntrinsics = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]])
+        self.setCalibrationFromIntrinsics(
+            width, height, newCamIntrinsics, newDistCoeffs
+        )
 
-        # really convince python we are doing the right thing
-        assert newCamIntrinsics is not None
-        assert newDistCoeffs is not None
+    def setCalibrationFromIntrinsics(
+        self,
+        width: int,
+        height: int,
+        newCamIntrinsics: np.ndarray,
+        newDistCoeffs: np.ndarray,
+    ) -> None:
 
         self.resWidth = width
         self.resHeight = height
         self.camIntrinsics = newCamIntrinsics
         self.distCoeffs = newDistCoeffs
 
+        # left, right, up, and down view planes
         p = [
             Translation3d(
                 1.0,
@@ -126,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:
@@ -171,31 +192,72 @@ class SimCameraProperties:
     def getLatencyStdDev(self) -> seconds:
         return self.latencyStdDev
 
-    def getContourAreaPercent(self, points: list[typing.Tuple[float, float]]) -> float:
-        return (
-            cv.contourArea(cv.convexHull(np.array(points))) / self.getResArea() * 100.0
-        )
+    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: typing.Tuple[int, int]) -> Rotation3d:
+    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(),
             self.getPixelYaw(point[0]).radians(),
         )
 
-    def getCorrectedPixelRot(self, point: typing.Tuple[float, float]) -> Rotation3d:
+    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]
@@ -209,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
@@ -226,9 +290,34 @@ class SimCameraProperties:
     def getVisibleLine(
         self, camRt: RotTrlTransform3d, a: Translation3d, b: Translation3d
     ) -> typing.Tuple[float | None, float | None]:
-        relA = camRt.apply(a)
-        relB = camRt.apply(b)
-
+        """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)
 
@@ -239,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:
@@ -247,39 +337,55 @@ 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
 
-            if not ipts[i]:
+            # // discard duplicate intersections
+            if ipts[i] is None:
                 continue
 
             for j in range(i - 1, 0 - 1):
@@ -293,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:
@@ -302,6 +409,7 @@ class SimCameraProperties:
                 else:
                     inter2 = inter
 
+        # // two viewplane intersections
         if not math.isnan(inter2):
             max_ = max(inter1, inter2)
             min_ = min(inter1, inter2)
@@ -310,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:
@@ -327,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(
@@ -342,22 +454,31 @@ 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
     def PI4_LIFECAM_320_240(cls) -> typing.Self:
         prop = cls()
-        prop.setCalibration(
+        prop.setCalibrationFromIntrinsics(
             320,
             240,
             newCamIntrinsics=np.array(
@@ -391,7 +512,7 @@ class SimCameraProperties:
     @classmethod
     def PI4_LIFECAM_640_480(cls) -> typing.Self:
         prop = cls()
-        prop.setCalibration(
+        prop.setCalibrationFromIntrinsics(
             640,
             480,
             newCamIntrinsics=np.array(
@@ -425,7 +546,7 @@ class SimCameraProperties:
     @classmethod
     def LL2_640_480(cls) -> typing.Self:
         prop = cls()
-        prop.setCalibration(
+        prop.setCalibrationFromIntrinsics(
             640,
             480,
             newCamIntrinsics=np.array(
@@ -459,7 +580,7 @@ class SimCameraProperties:
     @classmethod
     def LL2_960_720(cls) -> typing.Self:
         prop = cls()
-        prop.setCalibration(
+        prop.setCalibrationFromIntrinsics(
             960,
             720,
             newCamIntrinsics=np.array(
@@ -493,7 +614,7 @@ class SimCameraProperties:
     @classmethod
     def LL2_1280_720(cls) -> typing.Self:
         prop = cls()
-        prop.setCalibration(
+        prop.setCalibrationFromIntrinsics(
             1280,
             720,
             newCamIntrinsics=np.array(
@@ -527,7 +648,7 @@ class SimCameraProperties:
     @classmethod
     def OV9281_640_480(cls) -> typing.Self:
         prop = cls()
-        prop.setCalibration(
+        prop.setCalibrationFromIntrinsics(
             640,
             480,
             newCamIntrinsics=np.array(
@@ -561,7 +682,7 @@ class SimCameraProperties:
     @classmethod
     def OV9281_800_600(cls) -> typing.Self:
         prop = cls()
-        prop.setCalibration(
+        prop.setCalibrationFromIntrinsics(
             800,
             600,
             newCamIntrinsics=np.array(
@@ -595,7 +716,7 @@ class SimCameraProperties:
     @classmethod
     def OV9281_1280_720(cls) -> typing.Self:
         prop = cls()
-        prop.setCalibration(
+        prop.setCalibrationFromIntrinsics(
             1280,
             720,
             newCamIntrinsics=np.array(
@@ -629,7 +750,7 @@ class SimCameraProperties:
     @classmethod
     def OV9281_1920_1080(cls) -> typing.Self:
         prop = cls()
-        prop.setCalibration(
+        prop.setCalibrationFromIntrinsics(
             1920,
             1080,
             newCamIntrinsics=np.array(