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

Correct yet more python type hinting (#1555)

Browse Source
This commit is contained in:
James Ward
2024-11-13 03:17:27 +11:00
committed by GitHub
parent af03ae0a8b
commit 744e522aea
7 changed files with 38 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
photon-lib/py/photonlibpy/estimation/openCVHelp.py
View File

@@ -1,5 +1,5 @@
import math
from typing import Any, Tuple
from typing import Any
import cv2 as cv
import numpy as np
@@ -48,13 +48,13 @@ class OpenCVHelp:
)
@staticmethod
def avgPoint(points: list[Tuple[float, float]]) -> Tuple[float, float]:
def avgPoint(points: np.ndarray) -> np.ndarray:
x = 0.0
y = 0.0
for p in points:
x += p[0]
y += p[1]
return (x / len(points), y / len(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]:
@@ -126,6 +126,10 @@ class OpenCVHelp:
objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
alt: Transform3d | None = None
reprojectionError: cv.typing.MatLike | None = None
best: Transform3d = Transform3d()
alt: Transform3d | None = None
for tries in range(2):
retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
objectMat,
@@ -145,7 +149,9 @@ class OpenCVHelp:
OpenCVHelp.rVecToRotation(rvecs[1]),
)
if not math.isnan(reprojectionError[0, 0]):
if reprojectionError is not None and not math.isnan(
reprojectionError[0, 0]
):
break
else:
pt = imagePoints[0]
@@ -153,7 +159,7 @@ class OpenCVHelp:
pt[0, 1] -= 0.001
imagePoints[0] = pt
if math.isnan(reprojectionError[0, 0]):
if reprojectionError is None or math.isnan(reprojectionError[0, 0]):
print("SolvePNP_Square failed!")
return None

6
photon-lib/py/photonlibpy/networktables/NTTopicSet.py
View File

@@ -10,8 +10,10 @@ PhotonPipelineResult_TYPE_STRING = (
class NTTopicSet:
def __init__(self) -> None:
self.subTable = nt.NetworkTableInstance.getDefault()
def __init__(self, tableName: str, cameraName: str) -> None:
instance = nt.NetworkTableInstance.getDefault()
photonvision_root_table = instance.getTable(tableName)
self.subTable = photonvision_root_table.getSubTable(cameraName)
def updateEntries(self) -> None:
options = nt.PubSubOptions()

2
photon-lib/py/photonlibpy/photonCamera.py
View File

@@ -231,7 +231,7 @@ class PhotonCamera:
versionString = self.versionEntry.get(defaultValue="")
localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
remoteUUID = self._rawBytesEntry.getTopic().getProperty("message_uuid")
remoteUUID = str(self._rawBytesEntry.getTopic().getProperty("message_uuid"))
if not remoteUUID:
wpilib.reportWarning(

13
photon-lib/py/photonlibpy/simulation/photonCameraSim.py
View File

@@ -42,7 +42,6 @@ class PhotonCameraSim:
self.videoSimWireframeEnabled: bool = False
self.videoSimWireframeResolution: float = 0.1
self.videoSimProcEnabled: bool = True
self.ts = NTTopicSet()
self.heartbeatCounter: int = 0
self.nextNtEntryTime = int(wpilib.Timer.getFPGATimestamp() * 1e6)
self.tagLayout = robotpy_apriltag.loadAprilTagLayoutField(
@@ -101,7 +100,7 @@ class PhotonCameraSim:
(self.prop.getResWidth(), self.prop.getResHeight())
)
self.ts.subTable = self.cam._cameraTable
self.ts = NTTopicSet("photonvision", self.cam.getName())
self.ts.updateEntries()
# Handle this last explicitly for this function signature because the other constructor is called in the initialiser list
@@ -217,9 +216,7 @@ class PhotonCameraSim:
targets.sort(key=distance, reverse=True)
visibleTgts: list[
typing.Tuple[VisionTargetSim, list[typing.Tuple[float, float]]]
] = []
visibleTgts: list[typing.Tuple[VisionTargetSim, np.ndarray]] = []
detectableTgts: list[PhotonTrackedTarget] = []
camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
@@ -258,6 +255,7 @@ class PhotonCameraSim:
] * 4
t = (l + 1) % 4
b = (l + 1) % 4
r = 0
for i in range(4):
if i == l:
continue
@@ -271,14 +269,14 @@ class PhotonCameraSim:
if i != t and i != l and i != b:
r = i
rect = cv.RotatedRect(
center,
(center[0, 0], center[0, 1]),
(
imagePoints[r, 0, 0] - lc[0, 0],
imagePoints[b, 0, 1] - imagePoints[t, 0, 1],
),
-angles[r],
)
imagePoints = rect.points()
imagePoints = np.array([[p[0], p[1], p[2]] for p in rect.points()])
visibleTgts.append((tgt, imagePoints))
noisyTargetCorners = self.prop.estPixelNoise(imagePoints)
@@ -385,6 +383,7 @@ class PhotonCameraSim:
self.ts.targetSkewEntry.set(0.0, receiveTimestamp)
else:
bestTarget = result.getBestTarget()
assert bestTarget
self.ts.targetPitchEntry.set(bestTarget.getPitch(), receiveTimestamp)
self.ts.targetYawEntry.set(bestTarget.getYaw(), receiveTimestamp)

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

@@ -171,10 +171,8 @@ 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:
return cv.contourArea(cv.convexHull(points)) / self.getResArea() * 100.0
def getPixelYaw(self, pixelX: float) -> Rotation2d:
fx = self.camIntrinsics[0, 0]
@@ -188,14 +186,14 @@ class SimCameraProperties:
yOffset = cy - pixelY
return Rotation2d(fy, -yOffset)
def getPixelRot(self, point: typing.Tuple[int, int]) -> Rotation3d:
def getPixelRot(self, point: cv.typing.Point2f) -> Rotation3d:
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:
fx = self.camIntrinsics[0, 0]
cx = self.camIntrinsics[0, 2]
xOffset = cx - point[0]

15
photon-lib/py/photonlibpy/simulation/visionSystemSim.py
View File

@@ -84,7 +84,11 @@ class VisionSystemSim:
if robotToCamera is None:
return None
else:
return self.getRobotPose(time) + robotToCamera
pose = self.getRobotPose(time)
if pose:
return pose + robotToCamera
else:
return None
def adjustCamera(
self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d
@@ -167,7 +171,7 @@ class VisionSystemSim:
def getRobotPose(
self, timestamp: seconds = wpilib.Timer.getFPGATimestamp()
) -> Pose3d:
) -> Pose3d | None:
return self.robotPoseBuffer.sample(timestamp)
def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None:
@@ -216,9 +220,10 @@ class VisionSystemSim:
timestampCapture = timestampNt * 1.0e-6 - latency
lateRobotPose = self.getRobotPose(timestampCapture)
lateCameraPose = lateRobotPose + self.getRobotToCamera(
camSim, timestampCapture
)
robotToCamera = self.getRobotToCamera(camSim, timestampCapture)
if lateRobotPose is None or robotToCamera is None:
return None
lateCameraPose = lateRobotPose + robotToCamera
cameraPoses2d.append(lateCameraPose.toPose2d())
camResult = camSim.process(latency, lateCameraPose, allTargets)

12
photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py
View File

@@ -3,8 +3,6 @@ from typing import TYPE_CHECKING, ClassVar
from wpimath.geometry import Transform3d
from ..packet import Packet
if TYPE_CHECKING:
from .. import generated
@@ -27,14 +25,4 @@ class MultiTargetPNPResult:
estimatedPose: PnpResult = field(default_factory=PnpResult)
fiducialIDsUsed: list[int] = field(default_factory=list)
def createFromPacket(self, packet: Packet) -> Packet:
self.estimatedPose = PnpResult()
self.estimatedPose.createFromPacket(packet)
self.fiducialIDsUsed = []
for _ in range(MultiTargetPNPResult._MAX_IDS):
fidId = packet.decode16()
if fidId >= 0:
self.fiducialIDsUsed.append(fidId)
return packet
photonStruct: ClassVar["generated.MultiTargetPNPResultSerde"]