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

Break up masssive python overload hacks (#1573)

Browse Source 
What it says on the tin. This is all stuff from our initial effort to
port the sim things. Right now it is coupled to #1557 because this fixes
things up in that. Lets merge that one before dealing with this one
This commit is contained in:
Lucien Morey
2024-11-15 09:59:55 +11:00
committed by GitHub
parent 4dc4ae88de
commit dfed7e3621
6 changed files with 158 additions and 192 deletions
Download Patch File Download Diff File Expand all files Collapse all files

131
photon-lib/py/photonlibpy/estimation/targetModel.py
View File

@@ -8,86 +8,67 @@ from . import RotTrlTransform3d
class TargetModel:
def __init__(
self,
*,
width: meters | None = None,
height: meters | None = None,
length: meters | None = None,
diameter: meters | None = None,
verts: List[Translation3d] | None = None
):
def __init__(self):
self.vertices: List[Translation3d] = []
self.isPlanar = False
self.isSpherical = False
if (
width is not None
and height is not None
and length is None
and diameter is None
and verts is None
):
self.isPlanar = True
self.isSpherical = False
self.vertices = [
Translation3d(0.0, -width / 2.0, -height / 2.0),
Translation3d(0.0, width / 2.0, -height / 2.0),
Translation3d(0.0, width / 2.0, height / 2.0),
Translation3d(0.0, -width / 2.0, height / 2.0),
]
@classmethod
def createPlanar(cls, width: meters, height: meters) -> Self:
tm = cls()
return
tm.isPlanar = True
tm.isSpherical = False
tm.vertices = [
Translation3d(0.0, -width / 2.0, -height / 2.0),
Translation3d(0.0, width / 2.0, -height / 2.0),
Translation3d(0.0, width / 2.0, height / 2.0),
Translation3d(0.0, -width / 2.0, height / 2.0),
]
return tm
elif (
length is not None
and width is not None
and height is not None
and diameter is None
and verts is None
):
verts = [
Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
Translation3d(length / 2.0, width / 2.0, -height / 2.0),
Translation3d(length / 2.0, width / 2.0, height / 2.0),
Translation3d(length / 2.0, -width / 2.0, height / 2.0),
Translation3d(-length / 2.0, -width / 2.0, height / 2.0),
Translation3d(-length / 2.0, width / 2.0, height / 2.0),
Translation3d(-length / 2.0, width / 2.0, -height / 2.0),
Translation3d(-length / 2.0, -width / 2.0, -height / 2.0),
]
# Handle the rest of this in the "default" case
elif (
diameter is not None
and width is None
and height is None
and length is None
and verts is None
):
self.isPlanar = False
self.isSpherical = True
self.vertices = [
Translation3d(0.0, -diameter / 2.0, 0.0),
Translation3d(0.0, 0.0, -diameter / 2.0),
Translation3d(0.0, diameter / 2.0, 0.0),
Translation3d(0.0, 0.0, diameter / 2.0),
]
return
elif (
verts is not None
and width is None
and height is None
and length is None
and diameter is None
):
# Handle this in the "default" case
pass
else:
raise Exception("Not a valid overload")
@classmethod
def createCuboid(cls, length: meters, width: meters, height: meters) -> Self:
tm = cls()
verts = [
Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
Translation3d(length / 2.0, width / 2.0, -height / 2.0),
Translation3d(length / 2.0, width / 2.0, height / 2.0),
Translation3d(length / 2.0, -width / 2.0, height / 2.0),
Translation3d(-length / 2.0, -width / 2.0, height / 2.0),
Translation3d(-length / 2.0, width / 2.0, height / 2.0),
Translation3d(-length / 2.0, width / 2.0, -height / 2.0),
Translation3d(-length / 2.0, -width / 2.0, -height / 2.0),
]
# TODO maybe remove this if there is a better/preferred way
# make the python type checking gods happy
assert verts is not None
tm._common_construction(verts)
return tm
@classmethod
def createSpheroid(cls, diameter: meters) -> Self:
tm = cls()
tm.isPlanar = False
tm.isSpherical = True
tm.vertices = [
Translation3d(0.0, -diameter / 2.0, 0.0),
Translation3d(0.0, 0.0, -diameter / 2.0),
Translation3d(0.0, diameter / 2.0, 0.0),
Translation3d(0.0, 0.0, diameter / 2.0),
]
return tm
@classmethod
def createArbitrary(cls, verts: List[Translation3d]) -> Self:
tm = cls()
tm._common_construction(verts)
return tm
def _common_construction(self, verts: List[Translation3d]) -> None:
self.isSpherical = False
if len(verts) <= 2:
self.vertices = []
@@ -132,8 +113,8 @@ class TargetModel:
@classmethod
def AprilTag36h11(cls) -> Self:
return cls(width=6.5 * 0.0254, height=6.5 * 0.0254)
return cls.createPlanar(width=6.5 * 0.0254, height=6.5 * 0.0254)
@classmethod
def AprilTag16h5(cls) -> Self:
return cls(width=6.0 * 0.0254, height=6.0 * 0.0254)
return cls.createPlanar(width=6.0 * 0.0254, height=6.0 * 0.0254)

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

@@ -31,7 +31,7 @@ class PhotonCameraSim:
def __init__(
self,
camera: PhotonCamera,
props: SimCameraProperties | None = None,
props: SimCameraProperties = SimCameraProperties.PERFECT_90DEG(),
minTargetAreaPercent: float | None = None,
maxSightRange: meters | None = None,
):
@@ -48,30 +48,6 @@ class PhotonCameraSim:
self.nextNtEntryTime = int(wpilib.Timer.getFPGATimestamp() * 1e6)
self.tagLayout = AprilTagFieldLayout.loadField(AprilTagField.k2024Crescendo)
if (
camera is not None
and props is None
and minTargetAreaPercent is None
and maxSightRange is None
):
props = SimCameraProperties.PERFECT_90DEG()
elif (
camera is not None
and props is not None
and minTargetAreaPercent is not None
and maxSightRange is not None
):
pass
elif (
camera is not None
and props is not None
and minTargetAreaPercent is None
and maxSightRange is None
):
pass
else:
raise Exception("Invalid Constructor Called")
self.cam = camera
self.prop = props
self.setMinTargetAreaPixels(PhotonCameraSim.kDefaultMinAreaPx)
@@ -104,12 +80,7 @@ class PhotonCameraSim:
self.ts.updateEntries()
# Handle this last explicitly for this function signature because the other constructor is called in the initialiser list
if (
camera is not None
and props is not None
and minTargetAreaPercent is not None
and maxSightRange is not None
):
if minTargetAreaPercent is not None and maxSightRange is not None:
self.minTargetAreaPercent = minTargetAreaPercent
self.maxSightRange = maxSightRange

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

@@ -11,7 +11,7 @@ from ..estimation import RotTrlTransform3d
class SimCameraProperties:
def __init__(self, path: str | None = None, width: int = 0, height: int = 0):
def __init__(self):
self.resWidth: int = -1
self.resHeight: int = -1
self.camIntrinsics: np.ndarray = np.zeros((3, 3)) # [3,3]
@@ -24,59 +24,41 @@ 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")
self.setCalibrationFromFOV(960, 720, fovDiag=Rotation2d(math.radians(90.0)))
def setCalibration(
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...")
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)
newDistCoeffs = np.zeros((8, 1))
cx = width / 2.0 - 0.5
cy = height / 2.0 - 0.5
fx = cx / math.tan(fovWidth.radians() / 2.0)
fy = cy / math.tan(fovHeight.radians() / 2.0)
newCamIntrinsics = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]])
self.setCalibrationFromIntrinsics(
width, height, newCamIntrinsics, newDistCoeffs
)
def setCalibrationFromIntrinsics(
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
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")
if has_fov_args:
# really convince python we are doing the right thing
assert fovDiag is not 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..."
)
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)
newDistCoeffs = np.zeros((8, 1))
cx = width / 2.0 - 0.5
cy = height / 2.0 - 0.5
fx = cx / math.tan(fovWidth.radians() / 2.0)
fy = cy / math.tan(fovHeight.radians() / 2.0)
newCamIntrinsics = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]])
# really convince python we are doing the right thing
assert newCamIntrinsics is not None
assert newDistCoeffs is not None
newCamIntrinsics: np.ndarray,
newDistCoeffs: np.ndarray,
) -> None:
self.resWidth = width
self.resHeight = height
@@ -357,7 +339,7 @@ class SimCameraProperties:
@classmethod
def PI4_LIFECAM_320_240(cls) -> typing.Self:
prop = cls()
prop.setCalibration(
prop.setCalibrationFromIntrinsics(
320,
240,
newCamIntrinsics=np.array(
@@ -391,7 +373,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 +407,7 @@ class SimCameraProperties:
@classmethod
def LL2_640_480(cls) -> typing.Self:
prop = cls()
prop.setCalibration(
prop.setCalibrationFromIntrinsics(
640,
480,
newCamIntrinsics=np.array(
@@ -459,7 +441,7 @@ class SimCameraProperties:
@classmethod
def LL2_960_720(cls) -> typing.Self:
prop = cls()
prop.setCalibration(
prop.setCalibrationFromIntrinsics(
960,
720,
newCamIntrinsics=np.array(
@@ -493,7 +475,7 @@ class SimCameraProperties:
@classmethod
def LL2_1280_720(cls) -> typing.Self:
prop = cls()
prop.setCalibration(
prop.setCalibrationFromIntrinsics(
1280,
720,
newCamIntrinsics=np.array(
@@ -527,7 +509,7 @@ class SimCameraProperties:
@classmethod
def OV9281_640_480(cls) -> typing.Self:
prop = cls()
prop.setCalibration(
prop.setCalibrationFromIntrinsics(
640,
480,
newCamIntrinsics=np.array(
@@ -561,7 +543,7 @@ class SimCameraProperties:
@classmethod
def OV9281_800_600(cls) -> typing.Self:
prop = cls()
prop.setCalibration(
prop.setCalibrationFromIntrinsics(
800,
600,
newCamIntrinsics=np.array(
@@ -595,7 +577,7 @@ class SimCameraProperties:
@classmethod
def OV9281_1280_720(cls) -> typing.Self:
prop = cls()
prop.setCalibration(
prop.setCalibrationFromIntrinsics(
1280,
720,
newCamIntrinsics=np.array(
@@ -629,7 +611,7 @@ class SimCameraProperties:
@classmethod
def OV9281_1920_1080(cls) -> typing.Self:
prop = cls()
prop.setCalibration(
prop.setCalibrationFromIntrinsics(
1920,
1080,
newCamIntrinsics=np.array(

2
photon-lib/py/test/openCVHelp_test.py
View File

@@ -150,7 +150,7 @@ def test_SolvePNP_SQPNP():
# (for targets with arbitrary number of non-colinear points > 2)
target = VisionTargetSim(
Pose3d(Translation3d(5.0, 0.5, 1.0), Rotation3d(0.0, 0.0, math.pi)),
TargetModel(
TargetModel.createArbitrary(
verts=[
Translation3d(0.0, 0.0, 0.0),
Translation3d(1.0, 0.0, 0.0),

8
photon-lib/py/test/simCameraProperties_test.py
View File

@@ -10,16 +10,10 @@ from wpimath.geometry import Rotation2d, Translation3d
@pytest.fixture(autouse=True)
def scp() -> SimCameraProperties:
props = SimCameraProperties()
props.setCalibration(1000, 1000, fovDiag=Rotation2d(math.radians(90.0)))
props.setCalibrationFromFOV(1000, 1000, fovDiag=Rotation2d(math.radians(90.0)))
return props
def test_Constructor() -> None:
SimCameraProperties()
with pytest.raises(Exception):
SimCameraProperties("4774")
def test_GetPixelYaw(scp) -> None:
rot = scp.getPixelYaw(scp.getResWidth() / 2)
assert rot.degrees() == pytest.approx(0.0, abs=1.0)

76
photon-lib/py/test/visionSystemSim_test.py
View File

@@ -32,10 +32,14 @@ def test_VisibilityCupidShuffle() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
visionSysSim.addVisionTargets(
[VisionTargetSim(targetPose, TargetModel(width=1.0, height=1.0), 4774)]
[
VisionTargetSim(
targetPose, TargetModel.createPlanar(width=1.0, height=1.0), 4774
)
]
)
# To the right, to the right
@@ -89,10 +93,14 @@ def test_NotVisibleVert1() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
visionSysSim.addVisionTargets(
[VisionTargetSim(targetPose, TargetModel(width=3.0, height=3.0), 4774)]
[
VisionTargetSim(
targetPose, TargetModel.createPlanar(width=3.0, height=3.0), 4774
)
]
)
robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0))
@@ -120,9 +128,15 @@ def test_NotVisibleVert2() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, robotToCamera)
cameraSim.prop.setCalibration(4774, 4774, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.prop.setCalibrationFromFOV(
4774, 4774, fovDiag=Rotation2d.fromDegrees(80.0)
)
visionSysSim.addVisionTargets(
[VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)]
[
VisionTargetSim(
targetPose, TargetModel.createPlanar(width=0.5, height=0.5), 4774
)
]
)
robotPose = Pose2d(Translation2d(13.98, 0.0), Rotation2d.fromDegrees(5.0))
@@ -142,10 +156,14 @@ def test_NotVisibleTargetSize() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.setMinTargetAreaPixels(20.0)
visionSysSim.addVisionTargets(
[VisionTargetSim(targetPose, TargetModel(width=0.1, height=0.1), 4774)]
[
VisionTargetSim(
targetPose, TargetModel.createPlanar(width=0.1, height=0.1), 4774
)
]
)
robotPose = Pose2d(Translation2d(12.0, 0.0), Rotation2d.fromDegrees(5.0))
@@ -165,11 +183,15 @@ def test_NotVisibleTooFarLeds() -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.setMinTargetAreaPixels(1.0)
cameraSim.setMaxSightRange(10.0)
visionSysSim.addVisionTargets(
[VisionTargetSim(targetPose, TargetModel(width=1.0, height=1.0), 4774)]
[
VisionTargetSim(
targetPose, TargetModel.createPlanar(width=1.0, height=1.0), 4774
)
]
)
robotPose = Pose2d(Translation2d(10.0, 0.0), Rotation2d.fromDegrees(5.0))
@@ -194,10 +216,14 @@ def test_YawAngles(expected_yaw) -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.setMinTargetAreaPixels(0.0)
visionSysSim.addVisionTargets(
[VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)]
[
VisionTargetSim(
targetPose, TargetModel.createPlanar(width=0.5, height=0.5), 4774
)
]
)
robotPose = Pose2d(Translation2d(10.0, 0.0), Rotation2d.fromDegrees(expected_yaw))
@@ -224,10 +250,16 @@ def test_PitchAngles(expected_pitch) -> None:
camera = PhotonCamera("camera")
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(120.0))
cameraSim.prop.setCalibrationFromFOV(
640, 480, fovDiag=Rotation2d.fromDegrees(120.0)
)
cameraSim.setMinTargetAreaPixels(0.0)
visionSysSim.addVisionTargets(
[VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)]
[
VisionTargetSim(
targetPose, TargetModel.createPlanar(width=0.5, height=0.5), 4774
)
]
)
visionSysSim.adjustCamera(
cameraSim,
@@ -286,11 +318,17 @@ def test_distanceCalc(distParam, pitchParam, heightParam) -> None:
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(160.0))
cameraSim.prop.setCalibrationFromFOV(
640, 480, fovDiag=Rotation2d.fromDegrees(160.0)
)
cameraSim.setMinTargetAreaPixels(0.0)
visionSysSim.adjustCamera(cameraSim, robotToCamera)
visionSysSim.addVisionTargets(
[VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)]
[
VisionTargetSim(
targetPose, TargetModel.createPlanar(width=0.5, height=0.5), 4774
)
]
)
visionSysSim.update(robotPose)
@@ -316,7 +354,7 @@ def test_MultipleTargets() -> None:
camera = PhotonCamera("camera")
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
cameraSim.setMinTargetAreaPixels(20.0)
visionSysSim.addVisionTargets(
@@ -413,7 +451,7 @@ def test_PoseEstimation() -> None:
camera = PhotonCamera("camera")
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, Transform3d())
cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
cameraSim.setMinTargetAreaPixels(20.0)
tagList: list[AprilTag] = []
@@ -487,7 +525,7 @@ def test_PoseEstimationRotated() -> None:
camera = PhotonCamera("camera")
cameraSim = PhotonCameraSim(camera)
visionSysSim.addCamera(cameraSim, robotToCamera)
cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
cameraSim.setMinTargetAreaPixels(20.0)
tagList: list[AprilTag] = []