Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-21 01:01:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

sphinxify java docs for python code (#1575)

Browse Source
This commit is contained in:
Lucien Morey
2024-11-16 13:01:33 +11:00
committed by GitHub
parent 9bbf49bc6b
commit 04191efc51
10 changed files with 628 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

139
photon-lib/py/photonlibpy/simulation/simCameraProperties.py
View File

@@ -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