diff --git a/photon-lib/py/photonlibpy/estimation/targetModel.py b/photon-lib/py/photonlibpy/estimation/targetModel.py index ff1f2b8ed2..d8d8b0f3a9 100644 --- a/photon-lib/py/photonlibpy/estimation/targetModel.py +++ b/photon-lib/py/photonlibpy/estimation/targetModel.py @@ -8,86 +8,67 @@ 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), - ] - - return - - 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 createPlanar(cls, width: meters, height: meters) -> Self: + tm = cls() + + 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 + + @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), + ] + + 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) - # TODO maybe remove this if there is a better/preferred way - # make the python type checking gods happy - assert verts is not None + return tm + def _common_construction(self, verts: List[Translation3d]) -> None: self.isSpherical = False if len(verts) <= 2: self.vertices = [] @@ -132,8 +113,8 @@ def getIsSpherical(self) -> bool: @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) diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py index ae65ed9eb3..b50a917050 100644 --- a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py +++ b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py @@ -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 @@ def __init__( 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 @@ def __init__( 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 diff --git a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py index f4058d46b3..4e54e09900 100644 --- a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py +++ b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py @@ -11,7 +11,7 @@ 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 @@ def __init__(self, path: str | None = None, width: int = 0, height: int = 0): 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 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 setCalibration( + 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 @@ def PERFECT_90DEG(cls) -> typing.Self: @classmethod def PI4_LIFECAM_320_240(cls) -> typing.Self: prop = cls() - prop.setCalibration( + prop.setCalibrationFromIntrinsics( 320, 240, newCamIntrinsics=np.array( @@ -391,7 +373,7 @@ def PI4_LIFECAM_320_240(cls) -> typing.Self: @classmethod def PI4_LIFECAM_640_480(cls) -> typing.Self: prop = cls() - prop.setCalibration( + prop.setCalibrationFromIntrinsics( 640, 480, newCamIntrinsics=np.array( @@ -425,7 +407,7 @@ def PI4_LIFECAM_640_480(cls) -> typing.Self: @classmethod def LL2_640_480(cls) -> typing.Self: prop = cls() - prop.setCalibration( + prop.setCalibrationFromIntrinsics( 640, 480, newCamIntrinsics=np.array( @@ -459,7 +441,7 @@ def LL2_640_480(cls) -> typing.Self: @classmethod def LL2_960_720(cls) -> typing.Self: prop = cls() - prop.setCalibration( + prop.setCalibrationFromIntrinsics( 960, 720, newCamIntrinsics=np.array( @@ -493,7 +475,7 @@ def LL2_960_720(cls) -> typing.Self: @classmethod def LL2_1280_720(cls) -> typing.Self: prop = cls() - prop.setCalibration( + prop.setCalibrationFromIntrinsics( 1280, 720, newCamIntrinsics=np.array( @@ -527,7 +509,7 @@ def LL2_1280_720(cls) -> typing.Self: @classmethod def OV9281_640_480(cls) -> typing.Self: prop = cls() - prop.setCalibration( + prop.setCalibrationFromIntrinsics( 640, 480, newCamIntrinsics=np.array( @@ -561,7 +543,7 @@ def OV9281_640_480(cls) -> typing.Self: @classmethod def OV9281_800_600(cls) -> typing.Self: prop = cls() - prop.setCalibration( + prop.setCalibrationFromIntrinsics( 800, 600, newCamIntrinsics=np.array( @@ -595,7 +577,7 @@ def OV9281_800_600(cls) -> typing.Self: @classmethod def OV9281_1280_720(cls) -> typing.Self: prop = cls() - prop.setCalibration( + prop.setCalibrationFromIntrinsics( 1280, 720, newCamIntrinsics=np.array( @@ -629,7 +611,7 @@ def OV9281_1280_720(cls) -> typing.Self: @classmethod def OV9281_1920_1080(cls) -> typing.Self: prop = cls() - prop.setCalibration( + prop.setCalibrationFromIntrinsics( 1920, 1080, newCamIntrinsics=np.array( diff --git a/photon-lib/py/test/openCVHelp_test.py b/photon-lib/py/test/openCVHelp_test.py index a0bd30bed6..939bae5515 100644 --- a/photon-lib/py/test/openCVHelp_test.py +++ b/photon-lib/py/test/openCVHelp_test.py @@ -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), diff --git a/photon-lib/py/test/simCameraProperties_test.py b/photon-lib/py/test/simCameraProperties_test.py index 7db73a67f6..296b79e0f1 100644 --- a/photon-lib/py/test/simCameraProperties_test.py +++ b/photon-lib/py/test/simCameraProperties_test.py @@ -10,16 +10,10 @@ @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) diff --git a/photon-lib/py/test/visionSystemSim_test.py b/photon-lib/py/test/visionSystemSim_test.py index 25acb52f42..28b5c8f303 100644 --- a/photon-lib/py/test/visionSystemSim_test.py +++ b/photon-lib/py/test/visionSystemSim_test.py @@ -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] = []