From b3d74e56a016a39640d9b6f08c9b782f84b3e193 Mon Sep 17 00:00:00 2001 From: Lucien Morey Date: Mon, 11 Nov 2024 09:16:02 +1100 Subject: [PATCH] Add python simulation (#1532) --- .../py/photonlibpy/estimation/__init__.py | 5 + .../estimation/cameraTargetRelation.py | 25 + .../py/photonlibpy/estimation/openCVHelp.py | 200 ++++++ .../estimation/rotTrlTransform3d.py | 32 + .../py/photonlibpy/estimation/targetModel.py | 137 ++++ .../estimation/visionEstimation.py | 91 +++ .../photonlibpy/networktables/NTTopicSet.py | 64 ++ .../py/photonlibpy/networktables/__init__.py | 1 + photon-lib/py/photonlibpy/photonCamera.py | 5 +- .../py/photonlibpy/simulation/__init__.py | 5 + .../photonlibpy/simulation/photonCameraSim.py | 408 +++++++++++ .../simulation/simCameraProperties.py | 661 ++++++++++++++++++ .../py/photonlibpy/simulation/videoSimUtil.py | 2 + .../photonlibpy/simulation/visionSystemSim.py | 237 +++++++ .../photonlibpy/simulation/visionTargetSim.py | 50 ++ photon-lib/py/setup.py | 4 + photon-lib/py/test/visionSystemSim_test.py | 484 +++++++++++++ 17 files changed, 2407 insertions(+), 4 deletions(-) create mode 100644 photon-lib/py/photonlibpy/estimation/__init__.py create mode 100644 photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py create mode 100644 photon-lib/py/photonlibpy/estimation/openCVHelp.py create mode 100644 photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py create mode 100644 photon-lib/py/photonlibpy/estimation/targetModel.py create mode 100644 photon-lib/py/photonlibpy/estimation/visionEstimation.py create mode 100644 photon-lib/py/photonlibpy/networktables/NTTopicSet.py create mode 100644 photon-lib/py/photonlibpy/networktables/__init__.py create mode 100644 photon-lib/py/photonlibpy/simulation/__init__.py create mode 100644 photon-lib/py/photonlibpy/simulation/photonCameraSim.py create mode 100644 photon-lib/py/photonlibpy/simulation/simCameraProperties.py create mode 100644 photon-lib/py/photonlibpy/simulation/videoSimUtil.py create mode 100644 photon-lib/py/photonlibpy/simulation/visionSystemSim.py create mode 100644 photon-lib/py/photonlibpy/simulation/visionTargetSim.py create mode 100644 photon-lib/py/test/visionSystemSim_test.py diff --git a/photon-lib/py/photonlibpy/estimation/__init__.py b/photon-lib/py/photonlibpy/estimation/__init__.py new file mode 100644 index 000000000..f68a1e06a --- /dev/null +++ b/photon-lib/py/photonlibpy/estimation/__init__.py @@ -0,0 +1,5 @@ +from .cameraTargetRelation import CameraTargetRelation +from .openCVHelp import OpenCVHelp +from .rotTrlTransform3d import RotTrlTransform3d +from .targetModel import TargetModel +from .visionEstimation import VisionEstimation diff --git a/photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py b/photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py new file mode 100644 index 000000000..a084fdd10 --- /dev/null +++ b/photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py @@ -0,0 +1,25 @@ +import math + +from wpimath.geometry import Pose3d, Rotation2d, Transform3d +from wpimath.units import meters + + +class CameraTargetRelation: + def __init__(self, cameraPose: Pose3d, targetPose: Pose3d): + self.camPose = cameraPose + self.camToTarg = Transform3d(cameraPose, targetPose) + self.camToTargDist = self.camToTarg.translation().norm() + self.camToTargDistXY: meters = math.hypot( + self.camToTarg.translation().X(), self.camToTarg.translation().Y() + ) + self.camToTargYaw = Rotation2d(self.camToTarg.X(), self.camToTarg.Y()) + self.camToTargPitch = Rotation2d(self.camToTargDistXY, -self.camToTarg.Z()) + self.camToTargAngle = Rotation2d( + math.hypot(self.camToTargYaw.radians(), self.camToTargPitch.radians()) + ) + self.targToCam = Transform3d(targetPose, cameraPose) + self.targToCamYaw = Rotation2d(self.targToCam.X(), self.targToCam.Y()) + self.targToCamPitch = Rotation2d(self.camToTargDistXY, -self.targToCam.Z()) + self.targtoCamAngle = Rotation2d( + math.hypot(self.targToCamYaw.radians(), self.targToCamPitch.radians()) + ) diff --git a/photon-lib/py/photonlibpy/estimation/openCVHelp.py b/photon-lib/py/photonlibpy/estimation/openCVHelp.py new file mode 100644 index 000000000..79eb175af --- /dev/null +++ b/photon-lib/py/photonlibpy/estimation/openCVHelp.py @@ -0,0 +1,200 @@ +import math +from typing import Any, Tuple + +import cv2 as cv +import numpy as np +from wpimath.geometry import Rotation3d, Transform3d, Translation3d + +from ..targeting import PnpResult, TargetCorner +from .rotTrlTransform3d import RotTrlTransform3d + +NWU_TO_EDN = Rotation3d(np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]])) +EDN_TO_NWU = Rotation3d(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])) + + +class OpenCVHelp: + @staticmethod + def getMinAreaRect(points: np.ndarray) -> cv.RotatedRect: + return cv.RotatedRect(*cv.minAreaRect(points)) + + @staticmethod + def translationNWUtoEDN(trl: Translation3d) -> Translation3d: + return trl.rotateBy(NWU_TO_EDN) + + @staticmethod + def rotationNWUtoEDN(rot: Rotation3d) -> Rotation3d: + return -NWU_TO_EDN + (rot + NWU_TO_EDN) + + @staticmethod + def translationToTVec(translations: list[Translation3d]) -> np.ndarray: + retVal: list[list] = [] + for translation in translations: + trl = OpenCVHelp.translationNWUtoEDN(translation) + retVal.append([trl.X(), trl.Y(), trl.Z()]) + return np.array( + retVal, + dtype=np.float32, + ) + + @staticmethod + def rotationToRVec(rotation: Rotation3d) -> np.ndarray: + retVal: list[np.ndarray] = [] + rot = OpenCVHelp.rotationNWUtoEDN(rotation) + rotVec = rot.getQuaternion().toRotationVector() + retVal.append(rotVec) + return np.array( + retVal, + dtype=np.float32, + ) + + @staticmethod + def avgPoint(points: list[Tuple[float, float]]) -> Tuple[float, float]: + x = 0.0 + y = 0.0 + for p in points: + x += p[0] + y += p[1] + return (x / len(points), y / len(points)) + + @staticmethod + def pointsToTargetCorners(points: np.ndarray) -> list[TargetCorner]: + corners = [TargetCorner(p[0, 0], p[0, 1]) for p in points] + return corners + + @staticmethod + def cornersToPoints(corners: list[TargetCorner]) -> np.ndarray: + points = [[[c.x, c.y]] for c in corners] + return np.array(points) + + @staticmethod + def projectPoints( + cameraMatrix: np.ndarray, + distCoeffs: np.ndarray, + camRt: RotTrlTransform3d, + objectTranslations: list[Translation3d], + ) -> np.ndarray: + objectPoints = OpenCVHelp.translationToTVec(objectTranslations) + rvec = OpenCVHelp.rotationToRVec(camRt.getRotation()) + tvec = OpenCVHelp.translationToTVec( + [ + camRt.getTranslation(), + ] + ) + + pts, _ = cv.projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs) + return pts + + @staticmethod + def reorderCircular( + elements: list[Any] | np.ndarray, backwards: bool, shiftStart: int + ) -> list[Any]: + size = len(elements) + reordered = [] + dir = -1 if backwards else 1 + for i in range(size): + index = (i * dir + shiftStart * dir) % size + if index < 0: + index += size + reordered.append(elements[index]) + return reordered + + @staticmethod + def translationEDNToNWU(trl: Translation3d) -> Translation3d: + return trl.rotateBy(EDN_TO_NWU) + + @staticmethod + def rotationEDNToNWU(rot: Rotation3d) -> Rotation3d: + return -EDN_TO_NWU + (rot + EDN_TO_NWU) + + @staticmethod + def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d: + return OpenCVHelp.translationEDNToNWU(Translation3d(tvecInput)) + + @staticmethod + def rVecToRotation(rvecInput: np.ndarray) -> Rotation3d: + return OpenCVHelp.rotationEDNToNWU(Rotation3d(rvecInput)) + + @staticmethod + def solvePNP_Square( + cameraMatrix: np.ndarray, + distCoeffs: np.ndarray, + modelTrls: list[Translation3d], + imagePoints: np.ndarray, + ) -> PnpResult | None: + modelTrls = OpenCVHelp.reorderCircular(modelTrls, True, -1) + imagePoints = np.array(OpenCVHelp.reorderCircular(imagePoints, True, -1)) + objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls)) + + alt: Transform3d | None = None + for tries in range(2): + retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric( + objectMat, + imagePoints, + cameraMatrix, + distCoeffs, + flags=cv.SOLVEPNP_IPPE_SQUARE, + ) + + best = Transform3d( + OpenCVHelp.tVecToTranslation(tvecs[0]), + OpenCVHelp.rVecToRotation(rvecs[0]), + ) + if len(tvecs) > 1: + alt = Transform3d( + OpenCVHelp.tVecToTranslation(tvecs[1]), + OpenCVHelp.rVecToRotation(rvecs[1]), + ) + + if not math.isnan(reprojectionError[0, 0]): + break + else: + pt = imagePoints[0] + pt[0, 0] -= 0.001 + pt[0, 1] -= 0.001 + imagePoints[0] = pt + + if math.isnan(reprojectionError[0, 0]): + print("SolvePNP_Square failed!") + return None + + if alt: + return PnpResult( + best=best, + bestReprojErr=reprojectionError[0, 0], + alt=alt, + altReprojErr=reprojectionError[1, 0], + ambiguity=reprojectionError[0, 0] / reprojectionError[1, 0], + ) + else: + # We have no alternative so set it to best as well + return PnpResult( + best=best, + bestReprojErr=reprojectionError[0], + alt=best, + altReprojErr=reprojectionError[0], + ) + + @staticmethod + def solvePNP_SQPNP( + cameraMatrix: np.ndarray, + distCoeffs: np.ndarray, + modelTrls: list[Translation3d], + imagePoints: np.ndarray, + ) -> PnpResult | None: + objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls)) + + retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric( + objectMat, imagePoints, cameraMatrix, distCoeffs, flags=cv.SOLVEPNP_SQPNP + ) + + error = reprojectionError[0, 0] + best = Transform3d( + OpenCVHelp.tVecToTranslation(tvecs[0]), OpenCVHelp.rVecToRotation(rvecs[0]) + ) + + if math.isnan(error): + return None + + # We have no alternative so set it to best as well + result = PnpResult(best=best, bestReprojErr=error, alt=best, altReprojErr=error) + return result diff --git a/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py b/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py new file mode 100644 index 000000000..00664785e --- /dev/null +++ b/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py @@ -0,0 +1,32 @@ +from typing import Self + +from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d + + +class RotTrlTransform3d: + def __init__( + self, rot: Rotation3d = Rotation3d(), trl: Translation3d = Translation3d() + ): + self.rot = rot + self.trl = trl + + def inverse(self) -> Self: + invRot = -self.rot + invTrl = -(self.trl.rotateBy(invRot)) + return type(self)(invRot, invTrl) + + def getTransform(self) -> Transform3d: + return Transform3d(self.trl, self.rot) + + def getTranslation(self) -> Translation3d: + return self.trl + + def getRotation(self) -> Rotation3d: + return self.rot + + def apply(self, trlToApply: Translation3d) -> Translation3d: + return trlToApply.rotateBy(self.rot) + self.trl + + @classmethod + def makeRelativeTo(cls, pose: Pose3d) -> Self: + return cls(pose.rotation(), pose.translation()).inverse() diff --git a/photon-lib/py/photonlibpy/estimation/targetModel.py b/photon-lib/py/photonlibpy/estimation/targetModel.py new file mode 100644 index 000000000..e9006b50b --- /dev/null +++ b/photon-lib/py/photonlibpy/estimation/targetModel.py @@ -0,0 +1,137 @@ +import math +from typing import List, Self + +from wpimath.geometry import Pose3d, Rotation2d, Rotation3d, Translation3d +from wpimath.units import meters + +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 + ): + + 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") + + # TODO maybe remove this if there is a better/preferred way + # make the python type checking gods happy + assert verts is not None + + self.isSpherical = False + if len(verts) <= 2: + self.vertices: List[Translation3d] = [] + self.isPlanar = False + else: + cornersPlaner = True + for corner in verts: + if abs(corner.X() < 1e-4): + cornersPlaner = False + self.isPlanar = cornersPlaner + + self.vertices = verts + + def getFieldVertices(self, targetPose: Pose3d) -> List[Translation3d]: + basisChange = RotTrlTransform3d(targetPose.rotation(), targetPose.translation()) + + retVal = [] + + for vert in self.vertices: + retVal.append(basisChange.apply(vert)) + + return retVal + + @classmethod + def getOrientedPose(cls, tgtTrl: Translation3d, cameraTrl: Translation3d): + relCam = cameraTrl - tgtTrl + orientToCam = Rotation3d( + 0.0, + Rotation2d(math.hypot(relCam.X(), relCam.Y()), relCam.Z()).radians(), + Rotation2d(relCam.X(), relCam.Y()).radians(), + ) + return Pose3d(tgtTrl, orientToCam) + + def getVertices(self) -> List[Translation3d]: + return self.vertices + + def getIsPlanar(self) -> bool: + return self.isPlanar + + def getIsSpherical(self) -> bool: + return self.isSpherical + + @classmethod + def AprilTag36h11(cls) -> Self: + return cls(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) diff --git a/photon-lib/py/photonlibpy/estimation/visionEstimation.py b/photon-lib/py/photonlibpy/estimation/visionEstimation.py new file mode 100644 index 000000000..834df03d6 --- /dev/null +++ b/photon-lib/py/photonlibpy/estimation/visionEstimation.py @@ -0,0 +1,91 @@ +import numpy as np +from robotpy_apriltag import AprilTag, AprilTagFieldLayout +from wpimath.geometry import Pose3d, Transform3d, Translation3d + +from ..targeting import PhotonTrackedTarget, PnpResult, TargetCorner +from . import OpenCVHelp, TargetModel + + +class VisionEstimation: + @staticmethod + def getVisibleLayoutTags( + visTags: list[PhotonTrackedTarget], layout: AprilTagFieldLayout + ) -> list[AprilTag]: + retVal: list[AprilTag] = [] + for tag in visTags: + id = tag.getFiducialId() + maybePose = layout.getTagPose(id) + if maybePose: + tag = AprilTag() + tag.ID = id + tag.pose = maybePose + retVal.append(tag) + return retVal + + @staticmethod + def estimateCamPosePNP( + cameraMatrix: np.ndarray, + distCoeffs: np.ndarray, + visTags: list[PhotonTrackedTarget], + layout: AprilTagFieldLayout, + tagModel: TargetModel, + ) -> PnpResult | None: + if len(visTags) == 0: + return None + + corners: list[TargetCorner] = [] + knownTags: list[AprilTag] = [] + + for tgt in visTags: + id = tgt.getFiducialId() + maybePose = layout.getTagPose(id) + if maybePose: + tag = AprilTag() + tag.ID = id + tag.pose = maybePose + knownTags.append(tag) + currentCorners = tgt.getDetectedCorners() + if currentCorners: + corners += currentCorners + + if len(knownTags) == 0 or len(corners) == 0 or len(corners) % 4 != 0: + return None + + points = OpenCVHelp.cornersToPoints(corners) + + if len(knownTags) == 1: + camToTag = OpenCVHelp.solvePNP_Square( + cameraMatrix, distCoeffs, tagModel.getVertices(), points + ) + if not camToTag: + return None + + bestPose = knownTags[0].pose.transformBy(camToTag.best.inverse()) + altPose = Pose3d() + if camToTag.ambiguity != 0: + altPose = knownTags[0].pose.transformBy(camToTag.alt.inverse()) + + o = Pose3d() + result = PnpResult( + best=Transform3d(o, bestPose), + alt=Transform3d(o, altPose), + ambiguity=camToTag.ambiguity, + bestReprojErr=camToTag.bestReprojErr, + altReprojErr=camToTag.altReprojErr, + ) + return result + else: + objectTrls: list[Translation3d] = [] + for tag in knownTags: + verts = tagModel.getFieldVertices(tag.pose) + objectTrls += verts + + ret = OpenCVHelp.solvePNP_SQPNP( + cameraMatrix, distCoeffs, objectTrls, points + ) + if ret: + # Invert best/alt transforms + ret.best = ret.best.inverse() + ret.alt = ret.alt.inverse() + + return ret diff --git a/photon-lib/py/photonlibpy/networktables/NTTopicSet.py b/photon-lib/py/photonlibpy/networktables/NTTopicSet.py new file mode 100644 index 000000000..ae095b00a --- /dev/null +++ b/photon-lib/py/photonlibpy/networktables/NTTopicSet.py @@ -0,0 +1,64 @@ +import ntcore as nt +from wpimath.geometry import Transform3d + +from ..generated.PhotonPipelineResultSerde import PhotonPipelineResultSerde + +PhotonPipelineResult_TYPE_STRING = ( + "photonstruct:PhotonPipelineResult:" + PhotonPipelineResultSerde.MESSAGE_VERSION +) + + +class NTTopicSet: + + def __init__(self) -> None: + self.subTable = nt.NetworkTableInstance.getDefault() + + def updateEntries(self) -> None: + options = nt.PubSubOptions() + options.periodic = 0.01 + options.sendAll = True + self.rawBytesEntry = self.subTable.getRawTopic("rawBytes").publish( + PhotonPipelineResult_TYPE_STRING, options + ) + self.rawBytesEntry.getTopic().setProperty( + "message_uuid", PhotonPipelineResultSerde.MESSAGE_VERSION + ) + self.pipelineIndexPublisher = self.subTable.getIntegerTopic( + "pipelineIndexState" + ).publish() + self.pipelineIndexRequestSub = self.subTable.getIntegerTopic( + "pipelineIndexRequest" + ).subscribe(0) + + self.driverModePublisher = self.subTable.getBooleanTopic("driverMode").publish() + self.driverModeSubscriber = self.subTable.getBooleanTopic( + "driverModeRequest" + ).subscribe(False) + + self.driverModeSubscriber.getTopic().publish().setDefault(False) + + self.latencyMillisEntry = self.subTable.getDoubleTopic( + "latencyMillis" + ).publish() + self.hasTargetEntry = self.subTable.getBooleanTopic("hasTargets").publish() + + self.targetPitchEntry = self.subTable.getDoubleTopic("targetPitch").publish() + self.targetAreaEntry = self.subTable.getDoubleTopic("targetArea").publish() + self.targetYawEntry = self.subTable.getDoubleTopic("targetYaw").publish() + self.targetPoseEntry = self.subTable.getStructTopic( + "targetPose", Transform3d + ).publish() + self.targetSkewEntry = self.subTable.getDoubleTopic("targetSkew").publish() + + self.bestTargetPosX = self.subTable.getDoubleTopic("targetPixelsX").publish() + self.bestTargetPosY = self.subTable.getDoubleTopic("targetPixelsY").publish() + + self.heartbeatTopic = self.subTable.getIntegerTopic("heartbeat") + self.heartbeatPublisher = self.heartbeatTopic.publish() + + self.cameraIntrinsicsPublisher = self.subTable.getDoubleArrayTopic( + "cameraIntrinsics" + ).publish() + self.cameraDistortionPublisher = self.subTable.getDoubleArrayTopic( + "cameraDistortion" + ).publish() diff --git a/photon-lib/py/photonlibpy/networktables/__init__.py b/photon-lib/py/photonlibpy/networktables/__init__.py new file mode 100644 index 000000000..a404a3e71 --- /dev/null +++ b/photon-lib/py/photonlibpy/networktables/__init__.py @@ -0,0 +1 @@ +from .NTTopicSet import NTTopicSet diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index 6b224de4c..7081463c2 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -27,10 +27,7 @@ from wpilib import RobotController, Timer from .packet import Packet from .targeting.photonPipelineResult import PhotonPipelineResult -from .version import ( # type: ignore[import-untyped] - PHOTONLIB_VERSION, - PHOTONVISION_VERSION, -) +from .version import PHOTONLIB_VERSION # type: ignore[import-untyped] class VisionLEDMode(Enum): diff --git a/photon-lib/py/photonlibpy/simulation/__init__.py b/photon-lib/py/photonlibpy/simulation/__init__.py new file mode 100644 index 000000000..1d417000e --- /dev/null +++ b/photon-lib/py/photonlibpy/simulation/__init__.py @@ -0,0 +1,5 @@ +from .photonCameraSim import PhotonCameraSim +from .simCameraProperties import SimCameraProperties +from .videoSimUtil import VideoSimUtil +from .visionSystemSim import VisionSystemSim +from .visionTargetSim import VisionTargetSim diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py new file mode 100644 index 000000000..97bb3d870 --- /dev/null +++ b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py @@ -0,0 +1,408 @@ +import math +import typing + +import cscore as cs +import cv2 as cv +import numpy as np +import robotpy_apriltag +import wpilib +from wpimath.geometry import Pose3d, Transform3d +from wpimath.units import meters, seconds + +from ..estimation import OpenCVHelp, RotTrlTransform3d, TargetModel, VisionEstimation +from ..estimation.cameraTargetRelation import CameraTargetRelation +from ..networktables.NTTopicSet import NTTopicSet +from ..photonCamera import PhotonCamera +from ..targeting import ( + MultiTargetPNPResult, + PhotonPipelineMetadata, + PhotonPipelineResult, + PhotonTrackedTarget, + PnpResult, + TargetCorner, +) +from .simCameraProperties import SimCameraProperties +from .visionTargetSim import VisionTargetSim + + +class PhotonCameraSim: + kDefaultMinAreaPx: float = 100 + + def __init__( + self, + camera: PhotonCamera, + props: SimCameraProperties | None = None, + minTargetAreaPercent: float | None = None, + maxSightRange: meters | None = None, + ): + + self.minTargetAreaPercent: float = 0.0 + self.maxSightRange: float = 1.0e99 + self.videoSimRawEnabled: bool = False + 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( + robotpy_apriltag.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) + + # TODO Check fps is right + self.videoSimRaw = cs.CvSource( + self.cam.getName() + "-raw", + cs.VideoMode.PixelFormat.kGray, + self.prop.getResWidth(), + self.prop.getResHeight(), + 1, + ) + self.videoSimFrameRaw = np.zeros( + (self.prop.getResWidth(), self.prop.getResHeight()) + ) + + # TODO Check fps is right + self.videoSimProcessed = cs.CvSource( + self.cam.getName() + "-processed", + cs.VideoMode.PixelFormat.kGray, + self.prop.getResWidth(), + self.prop.getResHeight(), + 1, + ) + self.videoSimFrameProcessed = np.zeros( + (self.prop.getResWidth(), self.prop.getResHeight()) + ) + + self.ts.subTable = self.cam._cameraTable + 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 + ): + self.minTargetAreaPercent = minTargetAreaPercent + self.maxSightRange = maxSightRange + + def getCamera(self) -> PhotonCamera: + return self.cam + + def getMinTargetAreaPercent(self) -> float: + return self.minTargetAreaPercent + + def getMinTargetAreaPixels(self) -> float: + return self.minTargetAreaPercent / 100.0 * self.prop.getResArea() + + def getMaxSightRange(self) -> meters: + return self.maxSightRange + + def getVideoSimRaw(self) -> cs.CvSource: + return self.videoSimRaw + + def getVideoSimFrameRaw(self) -> np.ndarray: + return self.videoSimFrameRaw + + def canSeeTargetPose(self, camPose: Pose3d, target: VisionTargetSim) -> bool: + rel = CameraTargetRelation(camPose, target.getPose()) + return ( + ( + abs(rel.camToTargYaw.degrees()) + < self.prop.getHorizFOV().degrees() / 2.0 + and abs(rel.camToTargPitch.degrees()) + < self.prop.getVertFOV().degrees() / 2.0 + ) + and ( + not target.getModel().getIsPlanar() + or abs(rel.targtoCamAngle.degrees()) < 90 + ) + and rel.camToTarg.translation().norm() <= self.maxSightRange + ) + + def canSeeCorner(self, points: np.ndarray) -> bool: + assert points.shape[1] == 1 + assert points.shape[2] == 2 + for pt in points: + x = pt[0, 0] + y = pt[0, 1] + if ( + x < 0 + or x > self.prop.getResWidth() + or y < 0 + or y > self.prop.getResHeight() + ): + return False + + return True + + def consumeNextEntryTime(self) -> float | None: + now = int(wpilib.Timer.getFPGATimestamp() * 1e6) + timestamp = 0 + iter = 0 + while now >= self.nextNtEntryTime: + timestamp = int(self.nextNtEntryTime) + frameTime = int(self.prop.estSecUntilNextFrame() * 1e6) + self.nextNtEntryTime += frameTime + + iter += 1 + if iter > 50: + timestamp = now + self.nextNtEntryTime = now + frameTime + break + + if timestamp != 0: + return timestamp + + return None + + def setMinTargetAreaPercent(self, areaPercent: float) -> None: + self.minTargetAreaPercent = areaPercent + + def setMinTargetAreaPixels(self, areaPx: float) -> None: + self.minTargetAreaPercent = areaPx / self.prop.getResArea() * 100.0 + + def setMaxSightRange(self, range: meters) -> None: + self.maxSightRange = range + + def enableRawStream(self, enabled: bool) -> None: + raise Exception("Raw stream not implemented") + # self.videoSimRawEnabled = enabled + + def enableDrawWireframe(self, enabled: bool) -> None: + raise Exception("Wireframe not implemented") + # self.videoSimWireframeEnabled = enabled + + def setWireframeResolution(self, resolution: float) -> None: + self.videoSimWireframeResolution = resolution + + def enableProcessedStream(self, enabled: bool) -> None: + raise Exception("Processed stream not implemented") + # self.videoSimProcEnabled = enabled + + def process( + self, latency: seconds, cameraPose: Pose3d, targets: list[VisionTargetSim] + ) -> PhotonPipelineResult: + # Sort targets by distance to camera - furthest to closest + def distance(target: VisionTargetSim): + return target.getPose().translation().distance(cameraPose.translation()) + + targets.sort(key=distance, reverse=True) + + visibleTgts: list[ + typing.Tuple[VisionTargetSim, list[typing.Tuple[float, float]]] + ] = [] + detectableTgts: list[PhotonTrackedTarget] = [] + + camRt = RotTrlTransform3d.makeRelativeTo(cameraPose) + + for tgt in targets: + if not self.canSeeTargetPose(cameraPose, tgt): + continue + + fieldCorners = tgt.getFieldVertices() + isSpherical = tgt.getModel().getIsSpherical() + if isSpherical: + model = tgt.getModel() + fieldCorners = model.getFieldVertices( + TargetModel.getOrientedPose( + tgt.getPose().translation(), cameraPose.translation() + ) + ) + + imagePoints = OpenCVHelp.projectPoints( + self.prop.getIntrinsics(), + self.prop.getDistCoeffs(), + camRt, + fieldCorners, + ) + + if isSpherical: + center = OpenCVHelp.avgPoint(imagePoints) + l: int = 0 + for i in range(4): + if imagePoints[i, 0, 0] < imagePoints[l, 0, 0].x: + l = i + + lc = imagePoints[l] + angles = [ + 0.0, + ] * 4 + t = (l + 1) % 4 + b = (l + 1) % 4 + for i in range(4): + if i == l: + continue + ic = imagePoints[i] + angles[i] = math.atan2(lc[0, 1] - ic[0, 1], ic[0, 0] - lc[0, 0]) + if angles[i] >= angles[t]: + t = i + if angles[i] <= angles[b]: + b = i + for i in range(4): + if i != t and i != l and i != b: + r = i + rect = cv.RotatedRect( + center, + ( + imagePoints[r, 0, 0] - lc[0, 0], + imagePoints[b, 0, 1] - imagePoints[t, 0, 1], + ), + -angles[r], + ) + imagePoints = rect.points() + + visibleTgts.append((tgt, imagePoints)) + noisyTargetCorners = self.prop.estPixelNoise(imagePoints) + minAreaRect = OpenCVHelp.getMinAreaRect(noisyTargetCorners) + minAreaRectPts = minAreaRect.points() + centerPt = minAreaRect.center + centerRot = self.prop.getPixelRot(centerPt) + areaPercent = self.prop.getContourAreaPercent(noisyTargetCorners) + + if ( + not self.canSeeCorner(noisyTargetCorners) + or not areaPercent >= self.minTargetAreaPercent + ): + continue + + pnpSim: PnpResult | None = None + if tgt.fiducialId >= 0 and len(tgt.getFieldVertices()) == 4: + pnpSim = OpenCVHelp.solvePNP_Square( + self.prop.getIntrinsics(), + self.prop.getDistCoeffs(), + tgt.getModel().getVertices(), + noisyTargetCorners, + ) + + smallVec: list[TargetCorner] = [] + for corner in minAreaRectPts: + smallVec.append(TargetCorner(corner[0], corner[1])) + + cornersFloat = OpenCVHelp.pointsToTargetCorners(noisyTargetCorners) + + detectableTgts.append( + PhotonTrackedTarget( + yaw=math.degrees(-centerRot.Z()), + pitch=math.degrees(-centerRot.Y()), + area=areaPercent, + skew=math.degrees(centerRot.X()), + fiducialId=tgt.fiducialId, + detectedCorners=cornersFloat, + minAreaRectCorners=smallVec, + bestCameraToTarget=pnpSim.best if pnpSim else Transform3d(), + altCameraToTarget=pnpSim.alt if pnpSim else Transform3d(), + poseAmbiguity=pnpSim.ambiguity if pnpSim else -1, + ) + ) + + # Video streams disabled for now + if self.enableRawStream: + # VideoSimUtil::UpdateVideoProp(videoSimRaw, prop); + # cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()}; + # cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1); + # blankFrame.assignTo(videoSimFrameRaw); + pass + if self.enableProcessedStream: + # VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop); + pass + + multiTagResults: MultiTargetPNPResult | None = None + + visibleLayoutTags = VisionEstimation.getVisibleLayoutTags( + detectableTgts, self.tagLayout + ) + + if len(visibleLayoutTags) > 1: + usedIds = [tag.ID for tag in visibleLayoutTags] + usedIds.sort() + pnpResult = VisionEstimation.estimateCamPosePNP( + self.prop.getIntrinsics(), + self.prop.getDistCoeffs(), + detectableTgts, + self.tagLayout, + TargetModel.AprilTag36h11(), + ) + if pnpResult is not None: + multiTagResults = MultiTargetPNPResult(pnpResult, usedIds) + + self.heartbeatCounter += 1 + return PhotonPipelineResult( + metadata=PhotonPipelineMetadata( + self.heartbeatCounter, int(latency * 1e6), 1000000 + ), + targets=detectableTgts, + multitagResult=multiTagResults, + ) + + def submitProcessedFrame( + self, result: PhotonPipelineResult, receiveTimestamp: float | None + ): + if receiveTimestamp is None: + receiveTimestamp = wpilib.Timer.getFPGATimestamp() * 1e6 + receiveTimestamp = int(receiveTimestamp) + + self.ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp) + + newPacket = PhotonPipelineResult.photonStruct.pack(result) + self.ts.rawBytesEntry.set(newPacket.getData(), receiveTimestamp) + + hasTargets = result.hasTargets() + self.ts.hasTargetEntry.set(hasTargets, receiveTimestamp) + if not hasTargets: + self.ts.targetPitchEntry.set(0.0, receiveTimestamp) + self.ts.targetYawEntry.set(0.0, receiveTimestamp) + self.ts.targetAreaEntry.set(0.0, receiveTimestamp) + self.ts.targetPoseEntry.set(Transform3d(), receiveTimestamp) + self.ts.targetSkewEntry.set(0.0, receiveTimestamp) + else: + bestTarget = result.getBestTarget() + + self.ts.targetPitchEntry.set(bestTarget.getPitch(), receiveTimestamp) + self.ts.targetYawEntry.set(bestTarget.getYaw(), receiveTimestamp) + self.ts.targetAreaEntry.set(bestTarget.getArea(), receiveTimestamp) + self.ts.targetSkewEntry.set(bestTarget.getSkew(), receiveTimestamp) + + self.ts.targetPoseEntry.set( + bestTarget.getBestCameraToTarget(), receiveTimestamp + ) + + intrinsics = self.prop.getIntrinsics() + intrinsicsView = intrinsics.flatten().tolist() + self.ts.cameraIntrinsicsPublisher.set(intrinsicsView, receiveTimestamp) + + distortion = self.prop.getDistCoeffs() + distortionView = distortion.flatten().tolist() + self.ts.cameraDistortionPublisher.set(distortionView, receiveTimestamp) + + self.ts.heartbeatPublisher.set(self.heartbeatCounter, receiveTimestamp) + + self.ts.subTable.getInstance().flush() diff --git a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py new file mode 100644 index 000000000..1ab414607 --- /dev/null +++ b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py @@ -0,0 +1,661 @@ +import logging +import math +import typing + +import cv2 as cv +import numpy as np +from wpimath.geometry import Rotation2d, Rotation3d, Translation3d +from wpimath.units import hertz, seconds + +from ..estimation import RotTrlTransform3d + + +class SimCameraProperties: + def __init__(self, path: str | None = None, width: int = 0, height: int = 0): + self.resWidth: int = -1 + self.resHeight: int = -1 + self.camIntrinsics: np.ndarray = np.zeros((3, 3)) # [3,3] + self.distCoeffs: np.ndarray = np.zeros((8, 1)) # [8,1] + self.avgErrorPx: float = 0.0 + self.errorStdDevPx: float = 0.0 + self.frameSpeed: seconds = 0.0 + self.exposureTime: seconds = 0.0 + self.avgLatency: seconds = 0.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") + + 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 + + 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: + 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 + + self.resWidth = width + self.resHeight = height + self.camIntrinsics = newCamIntrinsics + self.distCoeffs = newDistCoeffs + + p = [ + Translation3d( + 1.0, + Rotation3d( + 0.0, + 0.0, + (self.getPixelYaw(0) + Rotation2d(math.pi / 2.0)).radians(), + ), + ), + Translation3d( + 1.0, + Rotation3d( + 0.0, + 0.0, + (self.getPixelYaw(width) + Rotation2d(math.pi / 2.0)).radians(), + ), + ), + Translation3d( + 1.0, + Rotation3d( + 0.0, + 0.0, + (self.getPixelPitch(0) + Rotation2d(math.pi / 2.0)).radians(), + ), + ), + Translation3d( + 1.0, + Rotation3d( + 0.0, + 0.0, + (self.getPixelPitch(height) + Rotation2d(math.pi / 2.0)).radians(), + ), + ), + ] + + self.viewplanes = [] + + for i in p: + self.viewplanes.append(np.array([i.X(), i.Y(), i.Z()])) + + def setCalibError(self, newAvgErrorPx: float, newErrorStdDevPx: float): + self.avgErrorPx = newAvgErrorPx + self.errorStdDevPx = newErrorStdDevPx + + def setFPS(self, fps: hertz): + self.frameSpeed = max(1.0 / fps, self.exposureTime) + + def setExposureTime(self, newExposureTime: seconds): + self.exposureTime = newExposureTime + self.frameSpeed = max(self.frameSpeed, self.exposureTime) + + def setAvgLatency(self, newAvgLatency: seconds): + self.vgLatency = newAvgLatency + + def setLatencyStdDev(self, newLatencyStdDev: seconds): + self.latencyStdDev = newLatencyStdDev + + def getResWidth(self) -> int: + return self.resWidth + + def getResHeight(self) -> int: + return self.resHeight + + def getResArea(self) -> int: + return self.resWidth * self.resHeight + + def getAspectRatio(self) -> float: + return 1.0 * self.resWidth / self.resHeight + + def getIntrinsics(self) -> np.ndarray: + return self.camIntrinsics + + def getDistCoeffs(self) -> np.ndarray: + return self.distCoeffs + + def getFPS(self) -> hertz: + return 1.0 / self.frameSpeed + + def getFrameSpeed(self) -> seconds: + return self.frameSpeed + + def getExposureTime(self) -> seconds: + return self.exposureTime + + def getAverageLatency(self) -> seconds: + return self.avgLatency + + 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 getPixelYaw(self, pixelX: float) -> Rotation2d: + fx = self.camIntrinsics[0, 0] + cx = self.camIntrinsics[0, 2] + xOffset = cx - pixelX + return Rotation2d(fx, xOffset) + + def getPixelPitch(self, pixelY: float) -> Rotation2d: + fy = self.camIntrinsics[1, 1] + cy = self.camIntrinsics[1, 2] + yOffset = cy - pixelY + return Rotation2d(fy, -yOffset) + + def getPixelRot(self, point: typing.Tuple[int, int]) -> Rotation3d: + return Rotation3d( + 0.0, + self.getPixelPitch(point[1]).radians(), + self.getPixelYaw(point[0]).radians(), + ) + + def getCorrectedPixelRot(self, point: typing.Tuple[float, float]) -> Rotation3d: + fx = self.camIntrinsics[0, 0] + cx = self.camIntrinsics[0, 2] + xOffset = cx - point[0] + + fy = self.camIntrinsics[1, 1] + cy = self.camIntrinsics[1, 2] + yOffset = cy - point[1] + + yaw = Rotation2d(fx, xOffset) + pitch = Rotation2d(fy / math.cos(math.atan(xOffset / fx)), -yOffset) + return Rotation3d(0.0, pitch.radians(), yaw.radians()) + + def getHorizFOV(self) -> Rotation2d: + left = self.getPixelYaw(0) + right = self.getPixelYaw(self.resWidth) + return left - right + + def getVertFOV(self) -> Rotation2d: + above = self.getPixelPitch(0) + below = self.getPixelPitch(self.resHeight) + return below - above + + def getDiagFOV(self) -> Rotation2d: + return Rotation2d( + math.hypot(self.getHorizFOV().radians(), self.getVertFOV().radians()) + ) + + def getVisibleLine( + self, camRt: RotTrlTransform3d, a: Translation3d, b: Translation3d + ) -> typing.Tuple[float | None, float | None]: + relA = camRt.apply(a) + relB = camRt.apply(b) + + if relA.X() <= 0.0 and relB.X() <= 0.0: + return (None, None) + + av = np.array([relA.X(), relA.Y(), relA.Z()]) + bv = np.array([relB.X(), relB.Y(), relB.Z()]) + abv = bv - av + + aVisible = True + bVisible = True + + for normal in self.viewplanes: + aVisibility = av.dot(normal) + if aVisibility < 0: + aVisible = False + + bVisibility = bv.dot(normal) + if bVisibility < 0: + bVisible = False + if aVisibility <= 0 and bVisibility <= 0: + return (None, None) + + if aVisible and bVisible: + return (0.0, 1.0) + + intersections = [float("nan"), float("nan"), float("nan"), float("nan")] + + # Optionally 3x1 vector + ipts: typing.List[np.ndarray | None] = [None, None, None, None] + + for i, normal in enumerate(self.viewplanes): + a_projn = (av.dot(normal) / normal.dot(normal)) * normal + + if abs(abv.dot(normal)) < 1.0e-5: + continue + intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn)) + + apv = intersections[i] * abv + intersectpt = av + apv + ipts[i] = intersectpt + + for j in range(1, len(self.viewplanes)): + if j == 0: + continue + oi = (i + j) % len(self.viewplanes) + onormal = self.viewplanes[oi] + if intersectpt.dot(onormal) < 0: + intersections[i] = float("nan") + ipts[i] = None + break + + if not ipts[i]: + continue + + for j in range(i - 1, 0 - 1): + oipt = ipts[j] + if not oipt: + continue + + diff = oipt - intersectpt + if abs(diff).max() < 1e-4: + intersections[i] = float("nan") + ipts[i] = None + break + + inter1 = float("nan") + inter2 = float("nan") + for inter in intersections: + if not math.isnan(inter): + if math.isnan(inter1): + inter1 = inter + else: + inter2 = inter + + if not math.isnan(inter2): + max_ = max(inter1, inter2) + min_ = min(inter1, inter2) + if aVisible: + min_ = 0 + if bVisible: + max_ = 1 + return (min_, max_) + elif not math.isnan(inter1): + if aVisible: + return (0, inter1) + if bVisible: + return (inter1, 1) + return (inter1, None) + else: + return (None, None) + + def estPixelNoise(self, points: np.ndarray) -> np.ndarray: + assert points.shape[1] == 1, points.shape + assert points.shape[2] == 2, points.shape + if self.avgErrorPx == 0 and self.errorStdDevPx == 0: + return points + + noisyPts: list[list] = [] + for p in points: + error = np.random.normal(self.avgErrorPx, self.errorStdDevPx, 1)[0] + errorAngle = np.random.uniform(-math.pi, math.pi) + noisyPts.append( + [ + [ + float(p[0, 0] + error * math.cos(errorAngle)), + float(p[0, 1] + error * math.sin(errorAngle)), + ] + ] + ) + retval = np.array(noisyPts, dtype=np.float32) + assert points.shape == retval.shape, retval + return retval + + def estLatency(self) -> seconds: + return max( + float(np.random.normal(self.avgLatency, self.latencyStdDev, 1)[0]), + 0.0, + ) + + def estSecUntilNextFrame(self) -> seconds: + return self.frameSpeed + max(0.0, self.estLatency() - self.frameSpeed) + + @classmethod + def PERFECT_90DEG(cls) -> typing.Self: + return cls() + + @classmethod + def PI4_LIFECAM_320_240(cls) -> typing.Self: + prop = cls() + prop.setCalibration( + 320, + 240, + newCamIntrinsics=np.array( + [ + [328.2733242048587, 0.0, 164.8190261141906], + [0.0, 318.0609794305216, 123.8633838438093], + [0.0, 0.0, 1.0], + ] + ), + newDistCoeffs=np.array( + [ + [ + 0.09957946553445934, + -0.9166265114485799, + 0.0019519890627236526, + -0.0036071725380870333, + 1.5627234622420942, + 0, + 0, + 0, + ] + ] + ), + ) + prop.setCalibError(0.21, 0.0124) + prop.setFPS(30.0) + prop.setAvgLatency(30.0e-3) + prop.setLatencyStdDev(10.0e-3) + return prop + + @classmethod + def PI4_LIFECAM_640_480(cls) -> typing.Self: + prop = cls() + prop.setCalibration( + 640, + 480, + newCamIntrinsics=np.array( + [ + [669.1428078983059, 0.0, 322.53377249329213], + [0.0, 646.9843137061716, 241.26567383784163], + [0.0, 0.0, 1.0], + ] + ), + newDistCoeffs=np.array( + [ + [ + 0.12788470750464645, + -1.2350335805796528, + 0.0024990767286192732, + -0.0026958287600230705, + 2.2951386729115537, + 0, + 0, + 0, + ] + ] + ), + ) + prop.setCalibError(0.26, 0.046) + prop.setFPS(15.0) + prop.setAvgLatency(65.0e-3) + prop.setLatencyStdDev(15.0e-3) + return prop + + @classmethod + def LL2_640_480(cls) -> typing.Self: + prop = cls() + prop.setCalibration( + 640, + 480, + newCamIntrinsics=np.array( + [ + [511.22843367007755, 0.0, 323.62049380211096], + [0.0, 514.5452336723849, 261.8827920543568], + [0.0, 0.0, 1.0], + ] + ), + newDistCoeffs=np.array( + [ + [ + 0.1917469998873756, + -0.5142936883324216, + 0.012461562046896614, + 0.0014084973492408186, + 0.35160648971214437, + 0, + 0, + 0, + ] + ] + ), + ) + prop.setCalibError(0.25, 0.05) + prop.setFPS(15.0) + prop.setAvgLatency(35.0e-3) + prop.setLatencyStdDev(8.0e-3) + return prop + + @classmethod + def LL2_960_720(cls) -> typing.Self: + prop = cls() + prop.setCalibration( + 960, + 720, + newCamIntrinsics=np.array( + [ + [769.6873145148892, 0.0, 486.1096609458122], + [0.0, 773.8164483705323, 384.66071662358354], + [0.0, 0.0, 1.0], + ] + ), + newDistCoeffs=np.array( + [ + [ + 0.189462064814501, + -0.49903003669627627, + 0.007468423590519429, + 0.002496885298683693, + 0.3443122090208624, + 0, + 0, + 0, + ] + ] + ), + ) + prop.setCalibError(0.35, 0.10) + prop.setFPS(10.0) + prop.setAvgLatency(50.0e-3) + prop.setLatencyStdDev(15.0e-3) + return prop + + @classmethod + def LL2_1280_720(cls) -> typing.Self: + prop = cls() + prop.setCalibration( + 1280, + 720, + newCamIntrinsics=np.array( + [ + [1011.3749416937393, 0.0, 645.4955139388737], + [0.0, 1008.5391755084075, 508.32877656020196], + [0.0, 0.0, 1.0], + ] + ), + newDistCoeffs=np.array( + [ + [ + 0.13730101577061535, + -0.2904345656989261, + 8.32475714507539e-4, + -3.694397782014239e-4, + 0.09487962227027584, + 0, + 0, + 0, + ] + ] + ), + ) + prop.setCalibError(0.37, 0.06) + prop.setFPS(7.0) + prop.setAvgLatency(60.0e-3) + prop.setLatencyStdDev(20.0e-3) + return prop + + @classmethod + def OV9281_640_480(cls) -> typing.Self: + prop = cls() + prop.setCalibration( + 640, + 480, + newCamIntrinsics=np.array( + [ + [627.1573807284262, 0, 307.79423851611824], + [0, 626.6621595938243, 219.02625533911998], + [0, 0, 1], + ] + ), + newDistCoeffs=np.array( + [ + [ + 0.054834081023049625, + -0.15994111706817074, + -0.0017587106009926158, + -0.0014671022483263552, + 0.049742166267499596, + 0, + 0, + 0, + ], + ] + ), + ) + prop.setCalibError(0.25, 0.05) + prop.setFPS(30.0) + prop.setAvgLatency(60.0e-3) + prop.setLatencyStdDev(20.0e-3) + return prop + + @classmethod + def OV9281_800_600(cls) -> typing.Self: + prop = cls() + prop.setCalibration( + 800, + 600, + newCamIntrinsics=np.array( + [ + [783.9467259105329, 0, 384.7427981451478], + [0, 783.3276994922804, 273.7828191739], + [0, 0, 1], + ] + ), + newDistCoeffs=np.array( + [ + [ + 0.054834081023049625, + -0.15994111706817074, + -0.0017587106009926158, + -0.0014671022483263552, + 0.049742166267499596, + 0, + 0, + 0, + ], + ] + ), + ) + prop.setCalibError(0.25, 0.05) + prop.setFPS(25.0) + prop.setAvgLatency(60.0e-3) + prop.setLatencyStdDev(20.0e-3) + return prop + + @classmethod + def OV9281_1280_720(cls) -> typing.Self: + prop = cls() + prop.setCalibration( + 1280, + 720, + newCamIntrinsics=np.array( + [ + [940.7360710926395, 0, 615.5884770322365], + [0, 939.9932393907364, 328.53938300868], + [0, 0, 1], + ] + ), + newDistCoeffs=np.array( + [ + [ + 0.054834081023049625, + -0.15994111706817074, + -0.0017587106009926158, + -0.0014671022483263552, + 0.049742166267499596, + 0, + 0, + 0, + ], + ] + ), + ) + prop.setCalibError(0.25, 0.05) + prop.setFPS(15.0) + prop.setAvgLatency(60.0e-3) + prop.setLatencyStdDev(20.0e-3) + return prop + + @classmethod + def OV9281_1920_1080(cls) -> typing.Self: + prop = cls() + prop.setCalibration( + 1920, + 1080, + newCamIntrinsics=np.array( + [ + [1411.1041066389591, 0, 923.3827155483548], + [0, 1409.9898590861046, 492.80907451301994], + [0, 0, 1], + ] + ), + newDistCoeffs=np.array( + [ + [ + 0.054834081023049625, + -0.15994111706817074, + -0.0017587106009926158, + -0.0014671022483263552, + 0.049742166267499596, + 0, + 0, + 0, + ], + ] + ), + ) + prop.setCalibError(0.25, 0.05) + prop.setFPS(10.0) + prop.setAvgLatency(60.0e-3) + prop.setLatencyStdDev(20.0e-3) + return prop diff --git a/photon-lib/py/photonlibpy/simulation/videoSimUtil.py b/photon-lib/py/photonlibpy/simulation/videoSimUtil.py new file mode 100644 index 000000000..2486d14d1 --- /dev/null +++ b/photon-lib/py/photonlibpy/simulation/videoSimUtil.py @@ -0,0 +1,2 @@ +class VideoSimUtil: + pass diff --git a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py b/photon-lib/py/photonlibpy/simulation/visionSystemSim.py new file mode 100644 index 000000000..839e01d5f --- /dev/null +++ b/photon-lib/py/photonlibpy/simulation/visionSystemSim.py @@ -0,0 +1,237 @@ +import typing + +import wpilib +from robotpy_apriltag import AprilTagFieldLayout +from wpilib import Field2d +from wpimath.geometry import Pose2d, Pose3d, Transform3d + +# TODO(auscompgeek): update import path when RobotPy re-exports are fixed +from wpimath.interpolation._interpolation import TimeInterpolatablePose3dBuffer +from wpimath.units import seconds + +from ..estimation import TargetModel +from .photonCameraSim import PhotonCameraSim +from .visionTargetSim import VisionTargetSim + + +class VisionSystemSim: + def __init__(self, visionSystemName: str): + self.dbgField: Field2d = Field2d() + self.bufferLength: seconds = 1.5 + + self.camSimMap: typing.Dict[str, PhotonCameraSim] = {} + self.camTrfMap: typing.Dict[PhotonCameraSim, TimeInterpolatablePose3dBuffer] = ( + {} + ) + self.robotPoseBuffer: TimeInterpolatablePose3dBuffer = ( + TimeInterpolatablePose3dBuffer(self.bufferLength) + ) + self.targetSets: typing.Dict[str, list[VisionTargetSim]] = {} + + self.tableName: str = "VisionSystemSim-" + visionSystemName + wpilib.SmartDashboard.putData(self.tableName + "/Sim Field", self.dbgField) + + def getCameraSim(self, name: str) -> PhotonCameraSim | None: + return self.camSimMap.get(name, None) + + def getCameraSims(self) -> list[PhotonCameraSim]: + return [*self.camSimMap.values()] + + def addCamera(self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d) -> None: + name = cameraSim.getCamera().getName() + if name not in self.camSimMap: + self.camSimMap[name] = cameraSim + self.camTrfMap[cameraSim] = TimeInterpolatablePose3dBuffer( + self.bufferLength + ) + self.camTrfMap[cameraSim].addSample( + wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera + ) + + def clearCameras(self) -> None: + self.camSimMap.clear() + self.camTrfMap.clear() + + def removeCamera(self, cameraSim: PhotonCameraSim) -> bool: + name = cameraSim.getCamera().getName() + if name in self.camSimMap: + del self.camSimMap[name] + return True + else: + return False + + def getRobotToCamera( + self, + cameraSim: PhotonCameraSim, + time: seconds = wpilib.Timer.getFPGATimestamp(), + ) -> Transform3d | None: + if cameraSim in self.camTrfMap: + trfBuffer = self.camTrfMap[cameraSim] + sample = trfBuffer.sample(time) + if sample is None: + return None + else: + return Transform3d(Pose3d(), sample) + else: + return None + + def getCameraPose( + self, + cameraSim: PhotonCameraSim, + time: seconds = wpilib.Timer.getFPGATimestamp(), + ) -> Pose3d | None: + robotToCamera = self.getRobotToCamera(cameraSim, time) + if robotToCamera is None: + return None + else: + return self.getRobotPose(time) + robotToCamera + + def adjustCamera( + self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d + ) -> bool: + if cameraSim in self.camTrfMap: + self.camTrfMap[cameraSim].addSample( + wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera + ) + return True + else: + return False + + def resetCameraTransforms(self, cameraSim: PhotonCameraSim | None = None) -> None: + now = wpilib.Timer.getFPGATimestamp() + + def resetSingleCamera(self, cameraSim: PhotonCameraSim) -> bool: + if cameraSim in self.camTrfMap: + trfBuffer = self.camTrfMap[cameraSim] + lastTrf = Transform3d(Pose3d(), trfBuffer.sample(now)) + trfBuffer.clear() + self.adjustCamera(cameraSim, lastTrf) + return True + else: + return False + + if cameraSim is None: + for camera in self.camTrfMap.keys(): + resetSingleCamera(self, camera) + else: + resetSingleCamera(self, cameraSim) + + def getVisionTargets(self, targetType: str | None = None) -> list[VisionTargetSim]: + if targetType is None: + all: list[VisionTargetSim] = [] + for targets in self.targetSets.values(): + for target in targets: + all.append(target) + return all + else: + return self.targetSets[targetType] + + def addVisionTargets( + self, targets: list[VisionTargetSim], targetType: str = "targets" + ) -> None: + if targetType not in self.targetSets: + self.targetSets[targetType] = targets + else: + self.targetSets[targetType] += targets + + def addAprilTags(self, layout: AprilTagFieldLayout) -> None: + targets: list[VisionTargetSim] = [] + for tag in layout.getTags(): + tag_pose = layout.getTagPose(tag.ID) + # TODO this was done to make the python gods happy. Confirm that this is desired or if types dont matter + assert tag_pose is not None + targets.append( + VisionTargetSim(tag_pose, TargetModel.AprilTag36h11(), tag.ID) + ) + self.addVisionTargets(targets, "apriltag") + + def clearVisionTargets(self) -> None: + self.targetSets.clear() + + def clearAprilTags(self) -> None: + self.removeVisionTargetType("apriltag") + + def removeVisionTargetType(self, targetType: str) -> None: + del self.targetSets[targetType] + + def removeVisionTargets( + self, targets: list[VisionTargetSim] + ) -> list[VisionTargetSim]: + removedList: list[VisionTargetSim] = [] + for target in targets: + for _, currentTargets in self.targetSets.items(): + if target in currentTargets: + removedList.append(target) + currentTargets.remove(target) + return removedList + + def getRobotPose( + self, timestamp: seconds = wpilib.Timer.getFPGATimestamp() + ) -> Pose3d: + return self.robotPoseBuffer.sample(timestamp) + + def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None: + if type(robotPose) is Pose2d: + robotPose = Pose3d(robotPose) + assert type(robotPose) is Pose3d + + self.robotPoseBuffer.clear() + self.robotPoseBuffer.addSample(wpilib.Timer.getFPGATimestamp(), robotPose) + + def getDebugField(self) -> Field2d: + return self.dbgField + + def update(self, robotPose: Pose2d | Pose3d) -> None: + if type(robotPose) is Pose2d: + robotPose = Pose3d(robotPose) + assert type(robotPose) is Pose3d + + for targetType, targets in self.targetSets.items(): + posesToAdd: list[Pose2d] = [] + for target in targets: + posesToAdd.append(target.getPose().toPose2d()) + self.dbgField.getObject(targetType).setPoses(posesToAdd) + + now = wpilib.Timer.getFPGATimestamp() + self.robotPoseBuffer.addSample(now, robotPose) + self.dbgField.setRobotPose(robotPose.toPose2d()) + + allTargets: list[VisionTargetSim] = [] + for targets in self.targetSets.values(): + for target in targets: + allTargets.append(target) + + visTgtPoses2d: list[Pose2d] = [] + cameraPoses2d: list[Pose2d] = [] + processed = False + for camSim in self.camSimMap.values(): + optTimestamp = camSim.consumeNextEntryTime() + if optTimestamp is None: + continue + else: + processed = True + + timestampNt = optTimestamp + latency = camSim.prop.estLatency() + timestampCapture = timestampNt * 1.0e-6 - latency + + lateRobotPose = self.getRobotPose(timestampCapture) + lateCameraPose = lateRobotPose + self.getRobotToCamera( + camSim, timestampCapture + ) + cameraPoses2d.append(lateCameraPose.toPose2d()) + + camResult = camSim.process(latency, lateCameraPose, allTargets) + camSim.submitProcessedFrame(camResult, timestampNt) + for target in camResult.getTargets(): + trf = target.getBestCameraToTarget() + if trf == Transform3d(): + continue + + visTgtPoses2d.append(lateCameraPose.transformBy(trf).toPose2d()) + + if processed: + self.dbgField.getObject("visibleTargetPoses").setPoses(visTgtPoses2d) + + if len(cameraPoses2d) != 0: + self.dbgField.getObject("cameras").setPoses(cameraPoses2d) diff --git a/photon-lib/py/photonlibpy/simulation/visionTargetSim.py b/photon-lib/py/photonlibpy/simulation/visionTargetSim.py new file mode 100644 index 000000000..97a0e8b17 --- /dev/null +++ b/photon-lib/py/photonlibpy/simulation/visionTargetSim.py @@ -0,0 +1,50 @@ +import math + +from wpimath.geometry import Pose3d, Translation3d + +from ..estimation.targetModel import TargetModel + + +class VisionTargetSim: + def __init__(self, pose: Pose3d, model: TargetModel, id: int = -1): + self.pose: Pose3d = pose + self.model: TargetModel = model + self.fiducialId: int = id + self.objDetClassId: int = -1 + self.objDetConf: float = -1.0 + + def __lt__(self, right) -> bool: + return self.pose.translation().norm() < right.pose.translation().norm() + + def __eq__(self, other) -> bool: + # Use 1 inch and 1 degree tolerance + return ( + abs(self.pose.translation().X() - other.getPose().translation().X()) + < 0.0254 + and abs(self.pose.translation().Y() - other.getPose().translation().Y()) + < 0.0254 + and abs(self.pose.translation().Z() - other.getPose().translation().Z()) + < 0.0254 + and abs(self.pose.rotation().X() - other.getPose().rotation().X()) + < math.radians(1) + and abs(self.pose.rotation().Y() - other.getPose().rotation().Y()) + < math.radians(1) + and abs(self.pose.rotation().Z() - other.getPose().rotation().Z()) + < math.radians(1) + and self.model.getIsPlanar() == other.getModel().getIsPlanar() + ) + + def setPose(self, newPose: Pose3d) -> None: + self.pose = newPose + + def setModel(self, newModel: TargetModel) -> None: + self.model = newModel + + def getPose(self) -> Pose3d: + return self.pose + + def getModel(self) -> TargetModel: + return self.model + + def getFieldVertices(self) -> list[Translation3d]: + return self.model.getFieldVertices(self.pose) diff --git a/photon-lib/py/setup.py b/photon-lib/py/setup.py index e3a28ea62..00671380a 100644 --- a/photon-lib/py/setup.py +++ b/photon-lib/py/setup.py @@ -57,10 +57,14 @@ setup( packages=find_packages(), version=versionString, install_requires=[ + "numpy~=1.25", "wpilib<2025,>=2024.0.0b2", "robotpy-wpimath<2025,>=2024.0.0b2", "robotpy-apriltag<2025,>=2024.0.0b2", + "robotpy-cscore<2025,>=2024.0.0.b2", "pyntcore<2025,>=2024.0.0b2", + "robotpy-opencv;platform_machine=='roborio'", + "opencv-python;platform_machine!='roborio'", ], description=descriptionStr, url="https://photonvision.org", diff --git a/photon-lib/py/test/visionSystemSim_test.py b/photon-lib/py/test/visionSystemSim_test.py new file mode 100644 index 000000000..4851f8190 --- /dev/null +++ b/photon-lib/py/test/visionSystemSim_test.py @@ -0,0 +1,484 @@ +import math + +import ntcore as nt +import pytest +from photonlibpy.estimation import TargetModel, VisionEstimation +from photonlibpy.photonCamera import PhotonCamera, setVersionCheckEnabled +from photonlibpy.simulation import PhotonCameraSim, VisionSystemSim, VisionTargetSim +from robotpy_apriltag import AprilTag, AprilTagFieldLayout +from wpimath.geometry import ( + Pose2d, + Pose3d, + Rotation2d, + Rotation3d, + Transform3d, + Translation2d, + Translation3d, +) +from wpimath.units import feetToMeters, meters + + +@pytest.fixture(autouse=True) +def setupCommon() -> None: + + nt.NetworkTableInstance.getDefault().startServer() + setVersionCheckEnabled(False) + + +def test_VisibilityCupidShuffle(): + + targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi)) + + visionSysSim = VisionSystemSim("Test") + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + visionSysSim.addCamera(cameraSim, Transform3d()) + + cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) + + visionSysSim.addVisionTargets( + [VisionTargetSim(targetPose, TargetModel(width=1.0, height=1.0), 4774)] + ) + + # To the right, to the right + robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(-70.0)) + visionSysSim.update(robotPose) + assert not camera.getLatestResult().hasTargets() + + # To the right, to the right + robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(-95.0)) + visionSysSim.update(robotPose) + assert not camera.getLatestResult().hasTargets() + + # To the left, to the left + robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(90.0)) + visionSysSim.update(robotPose) + assert not camera.getLatestResult().hasTargets() + + # To the left, to the left + robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(65.0)) + visionSysSim.update(robotPose) + assert not camera.getLatestResult().hasTargets() + + # Now kick, now kick + robotPose = Pose2d(Translation2d(2.0, 0.0), Rotation2d.fromDegrees(5.0)) + visionSysSim.update(robotPose) + assert camera.getLatestResult().hasTargets() + + # Now kick, now kick + robotPose = Pose2d(Translation2d(2.0, 0.0), Rotation2d.fromDegrees(-5.0)) + visionSysSim.update(robotPose) + assert camera.getLatestResult().hasTargets() + + # Now walk it by yourself + robotPose = Pose2d(Translation2d(2.0, 0.0), Rotation2d.fromDegrees(-179.0)) + visionSysSim.update(robotPose) + assert not camera.getLatestResult().hasTargets() + + # Now walk it by yourself + visionSysSim.adjustCamera( + cameraSim, Transform3d(Translation3d(), Rotation3d(0, 0, math.pi)) + ) + visionSysSim.update(robotPose) + assert camera.getLatestResult().hasTargets() + + +def test_NotVisibleVert1(): + + targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi)) + + visionSysSim = VisionSystemSim("Test") + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + visionSysSim.addCamera(cameraSim, Transform3d()) + + cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) + + visionSysSim.addVisionTargets( + [VisionTargetSim(targetPose, TargetModel(width=3.0, height=3.0), 4774)] + ) + + robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0)) + + visionSysSim.update(robotPose) + assert camera.getLatestResult().hasTargets() + + visionSysSim.adjustCamera( + cameraSim, + Transform3d(Translation3d(0.0, 0.0, 5000.0), Rotation3d(0.0, 0.0, math.pi)), + ) + visionSysSim.update(robotPose) + assert not camera.getLatestResult().hasTargets() + + +def test_NotVisibleVert2(): + + targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi)) + + robotToCamera = Transform3d( + Translation3d(0.0, 0.0, 1.0), Rotation3d(0.0, -math.pi / 4.0, 0.0) + ) + + visionSysSim = VisionSystemSim("Test") + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + visionSysSim.addCamera(cameraSim, robotToCamera) + + cameraSim.prop.setCalibration(4774, 4774, fovDiag=Rotation2d.fromDegrees(80.0)) + visionSysSim.addVisionTargets( + [VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)] + ) + + robotPose = Pose2d(Translation2d(13.98, 0.0), Rotation2d.fromDegrees(5.0)) + visionSysSim.update(robotPose) + assert camera.getLatestResult().hasTargets() + + robotPose = Pose2d(Translation2d(0.0, 0.0), Rotation2d.fromDegrees(5.0)) + visionSysSim.update(robotPose) + assert not camera.getLatestResult().hasTargets() + + +def test_NotVisibleTargetSize(): + targetPose = Pose3d(Translation3d(15.98, 0.0, 1.0), Rotation3d(0, 0, math.pi)) + + visionSysSim = VisionSystemSim("Test") + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + visionSysSim.addCamera(cameraSim, Transform3d()) + + cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) + cameraSim.setMinTargetAreaPixels(20.0) + visionSysSim.addVisionTargets( + [VisionTargetSim(targetPose, TargetModel(width=0.1, height=0.1), 4774)] + ) + + robotPose = Pose2d(Translation2d(12.0, 0.0), Rotation2d.fromDegrees(5.0)) + visionSysSim.update(robotPose) + assert camera.getLatestResult().hasTargets() + + robotPose = Pose2d(Translation2d(0.0, 0.0), Rotation2d.fromDegrees(5.0)) + visionSysSim.update(robotPose) + assert not camera.getLatestResult().hasTargets() + + +def test_NotVisibleTooFarLeds(): + + targetPose = Pose3d(Translation3d(15.98, 0.0, 1.0), Rotation3d(0, 0, math.pi)) + + visionSysSim = VisionSystemSim("Test") + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + visionSysSim.addCamera(cameraSim, Transform3d()) + + cameraSim.prop.setCalibration(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)] + ) + + robotPose = Pose2d(Translation2d(10.0, 0.0), Rotation2d.fromDegrees(5.0)) + visionSysSim.update(robotPose) + assert camera.getLatestResult().hasTargets() + + robotPose = Pose2d(Translation2d(0.0, 0.0), Rotation2d.fromDegrees(5.0)) + visionSysSim.update(robotPose) + assert not camera.getLatestResult().hasTargets() + + +@pytest.mark.parametrize( + "expected_yaw", [-10.0, -5.0, -2.0, -1.0, 0.0, 5.0, 7.0, 10.23] +) +def test_YawAngles(expected_yaw): + + targetPose = Pose3d( + Translation3d(15.98, 0.0, 1.0), Rotation3d(0.0, 0.0, 3.0 * math.pi / 4.0) + ) + + visionSysSim = VisionSystemSim("Test") + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + + visionSysSim.addCamera(cameraSim, Transform3d()) + cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) + cameraSim.setMinTargetAreaPixels(0.0) + visionSysSim.addVisionTargets( + [VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)] + ) + + robotPose = Pose2d(Translation2d(10.0, 0.0), Rotation2d.fromDegrees(expected_yaw)) + visionSysSim.update(robotPose) + + result = camera.getLatestResult() + + assert result.hasTargets() + assert result.getBestTarget().getYaw() == pytest.approx(expected_yaw, abs=0.25) + + +@pytest.mark.parametrize( + "expected_pitch", [-10.0, -5.0, -2.0, -1.0, 0.0, 5.0, 7.0, 10.23] +) +def test_PitchAngles(expected_pitch): + + targetPose = Pose3d( + Translation3d(15.98, 0.0, 0.0), Rotation3d(0, 0, 3.0 * math.pi / 4.0) + ) + robotPose = Pose2d( + Translation2d(10.0, 0.0), Rotation2d.fromDegrees(-expected_pitch) + ) + visionSysSim = VisionSystemSim("Test") + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + visionSysSim.addCamera(cameraSim, Transform3d()) + cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(120.0)) + cameraSim.setMinTargetAreaPixels(0.0) + visionSysSim.addVisionTargets( + [VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)] + ) + visionSysSim.adjustCamera( + cameraSim, + Transform3d( + Translation3d(), Rotation3d(0.0, math.radians(expected_pitch), 0.0) + ), + ) + visionSysSim.update(robotPose) + + result = camera.getLatestResult() + + assert result.hasTargets() + assert result.getBestTarget().getPitch() == pytest.approx(expected_pitch, abs=0.25) + + +@pytest.mark.parametrize( + "distParam, pitchParam, heightParam", + [ + (5, -15.98, 0), + (6, -15.98, 1), + (10, -15.98, 0), + (15, -15.98, 2), + (19.95, -15.98, 0), + (20, -15.98, 0), + (5, -42, 1), + (6, -42, 0), + (10, -42, 2), + (15, -42, 0.5), + (19.42, -15.98, 0), + (20, -42, 0), + (5, -55, 2), + (6, -55, 0), + (10, -54, 2.2), + (15, -53, 0), + (19.52, -15.98, 1.1), + ], +) +def test_distanceCalc(distParam, pitchParam, heightParam): + distParam = feetToMeters(distParam) + pitchParam = math.radians(pitchParam) + heightParam = feetToMeters(heightParam) + + targetPose = Pose3d( + Translation3d(15.98, 0.0, 1.0), Rotation3d(0.0, 0.0, 0.98 * math.pi) + ) + robotPose = Pose3d(Translation3d(15.98 - distParam, 0.0, 0.0), Rotation3d()) + robotToCamera = Transform3d( + Translation3d(0.0, 0.0, heightParam), Rotation3d(0.0, pitchParam, 0.0) + ) + + visionSysSim = VisionSystemSim( + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife" + ) + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + + visionSysSim.addCamera(cameraSim, Transform3d()) + cameraSim.prop.setCalibration(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)] + ) + visionSysSim.update(robotPose) + + result = camera.getLatestResult() + + assert result.hasTargets() + + target = result.getBestTarget() + + assert target.getYaw() == pytest.approx(0.0, abs=0.5) + + # TODO Enable when PhotonUtils is ported + # dist = PhotonUtils.calculateDistanceToTarget( + # robotToCamera.Z(), targetPose.Z(), -pitchParam, math.degrees(target.getPitch()) + # ) + # assert dist == pytest.approx(distParam, abs=0.25) + + +def test_MultipleTargets(): + targetPoseL = Pose3d(Translation3d(15.98, 2.0, 0.0), Rotation3d(0.0, 0.0, math.pi)) + targetPoseC = Pose3d(Translation3d(15.98, 0.0, 0.0), Rotation3d(0.0, 0.0, math.pi)) + targetPoseR = Pose3d(Translation3d(15.98, -2.0, 0.0), Rotation3d(0.0, 0.0, math.pi)) + + visionSysSim = VisionSystemSim("Test") + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + visionSysSim.addCamera(cameraSim, Transform3d()) + cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) + cameraSim.setMinTargetAreaPixels(20.0) + + visionSysSim.addVisionTargets( + [ + VisionTargetSim( + targetPoseL.transformBy( + Transform3d(Translation3d(0, 0, 0), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 1, + ), + VisionTargetSim( + targetPoseC.transformBy( + Transform3d(Translation3d(0, 0, 0), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 2, + ), + VisionTargetSim( + targetPoseR.transformBy( + Transform3d(Translation3d(0, 0, 0), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 3, + ), + VisionTargetSim( + targetPoseL.transformBy( + Transform3d(Translation3d(0, 0, 1), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 4, + ), + VisionTargetSim( + targetPoseC.transformBy( + Transform3d(Translation3d(0, 0, 1), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 5, + ), + VisionTargetSim( + targetPoseR.transformBy( + Transform3d(Translation3d(0, 0, 1), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 6, + ), + VisionTargetSim( + targetPoseL.transformBy( + Transform3d(Translation3d(0, 0, 0.5), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 7, + ), + VisionTargetSim( + targetPoseC.transformBy( + Transform3d(Translation3d(0, 0, 0.5), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 8, + ), + VisionTargetSim( + targetPoseL.transformBy( + Transform3d(Translation3d(0, 0, 0.75), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 9, + ), + VisionTargetSim( + targetPoseR.transformBy( + Transform3d(Translation3d(0, 0, 0.75), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 10, + ), + VisionTargetSim( + targetPoseL.transformBy( + Transform3d(Translation3d(0, 0, 0.25), Rotation3d()) + ), + TargetModel.AprilTag16h5(), + 11, + ), + ] + ) + robotPose = Pose2d(Translation2d(6.0, 0.0), Rotation2d.fromDegrees(0.25)) + visionSysSim.update(robotPose) + res = camera.getLatestResult() + assert res.hasTargets() + tgtList = res.getTargets() + assert len(tgtList) == 11 + + +def test_PoseEstimation(): + visionSysSim = VisionSystemSim("Test") + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + visionSysSim.addCamera(cameraSim, Transform3d()) + cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(90.0)) + cameraSim.setMinTargetAreaPixels(20.0) + + tagList: list[AprilTag] = [] + at0 = AprilTag() + at0.ID = 0 + at0.pose = Pose3d(12.0, 3.0, 1.0, Rotation3d(0.0, 0.0, math.pi)) + tagList.append(at0) + at1 = AprilTag() + at1.ID = 1 + at1.pose = Pose3d(12.0, 1.0, -1.0, Rotation3d(0.0, 0.0, math.pi)) + tagList.append(at1) + at2 = AprilTag() + at2.ID = 2 + at2.pose = Pose3d(11.0, 0.0, 2.0, Rotation3d(0.0, 0.0, math.pi)) + tagList.append(at2) + + fieldLength: meters = 54.0 + fieldWidth: meters = 27.0 + layout = AprilTagFieldLayout(tagList, fieldLength, fieldWidth) + robotPose = Pose2d(Translation2d(5.0, 1.0), Rotation2d.fromDegrees(5.0)) + visionSysSim.addVisionTargets( + [VisionTargetSim(tagList[0].pose, TargetModel.AprilTag16h5(), 0)] + ) + + visionSysSim.update(robotPose) + + camEigen = cameraSim.prop.getIntrinsics() + distEigen = cameraSim.prop.getDistCoeffs() + + camResults = camera.getLatestResult() + targets = camResults.getTargets() + results = VisionEstimation.estimateCamPosePNP( + camEigen, distEigen, targets, layout, TargetModel.AprilTag16h5() + ) + assert results is not None + pose: Pose3d = Pose3d() + results.best + assert pose.X() == pytest.approx(5.0, abs=0.01) + assert pose.Y() == pytest.approx(1.0, abs=0.01) + assert pose.Z() == pytest.approx(0.0, abs=0.01) + assert pose.rotation().Z() == pytest.approx(math.radians(5.0), abs=0.01) + + visionSysSim.addVisionTargets( + [VisionTargetSim(tagList[1].pose, TargetModel.AprilTag16h5(), 1)] + ) + visionSysSim.addVisionTargets( + [VisionTargetSim(tagList[2].pose, TargetModel.AprilTag16h5(), 2)] + ) + visionSysSim.update(robotPose) + + camResults2 = camera.getLatestResult() + targets2 = camResults2.getTargets() + results2 = VisionEstimation.estimateCamPosePNP( + camEigen, distEigen, targets2, layout, TargetModel.AprilTag16h5() + ) + assert results2 is not None + pose2 = Pose3d() + results2.best + + assert pose2.X() == pytest.approx(robotPose.X(), abs=0.01) + assert pose2.Y() == pytest.approx(robotPose.Y(), abs=0.01) + assert pose2.Z() == pytest.approx(0.0, abs=0.01) + assert pose2.rotation().Z() == pytest.approx(math.radians(5.0), abs=0.01)