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

Add python simulation (#1532)

Browse Source
This commit is contained in:
Lucien Morey
2024-11-11 09:16:02 +11:00
committed by GitHub
parent b5d48a6503
commit b3d74e56a0
17 changed files with 2407 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
photon-lib/py/photonlibpy/estimation/__init__.py Normal file
View File

@@ -0,0 +1,5 @@
from .cameraTargetRelation import CameraTargetRelation
from .openCVHelp import OpenCVHelp
from .rotTrlTransform3d import RotTrlTransform3d
from .targetModel import TargetModel
from .visionEstimation import VisionEstimation

25
photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py Normal file
View File

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

200
photon-lib/py/photonlibpy/estimation/openCVHelp.py Normal file
View File

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

32
photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py Normal file
View File

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

137
photon-lib/py/photonlibpy/estimation/targetModel.py Normal file
View File

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

91
photon-lib/py/photonlibpy/estimation/visionEstimation.py Normal file
View File

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

64
photon-lib/py/photonlibpy/networktables/NTTopicSet.py Normal file
View File

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

1
photon-lib/py/photonlibpy/networktables/__init__.py Normal file
View File

@@ -0,0 +1 @@
from .NTTopicSet import NTTopicSet

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

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

5
photon-lib/py/photonlibpy/simulation/__init__.py Normal file
View File

@@ -0,0 +1,5 @@
from .photonCameraSim import PhotonCameraSim
from .simCameraProperties import SimCameraProperties
from .videoSimUtil import VideoSimUtil
from .visionSystemSim import VisionSystemSim
from .visionTargetSim import VisionTargetSim

408
photon-lib/py/photonlibpy/simulation/photonCameraSim.py Normal file
View File

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

661
photon-lib/py/photonlibpy/simulation/simCameraProperties.py Normal file
View File

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

2
photon-lib/py/photonlibpy/simulation/videoSimUtil.py Normal file
View File

@@ -0,0 +1,2 @@
class VideoSimUtil:
pass

237
photon-lib/py/photonlibpy/simulation/visionSystemSim.py Normal file
View File

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

50
photon-lib/py/photonlibpy/simulation/visionTargetSim.py Normal file
View File

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

4
photon-lib/py/setup.py
View File

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

484
photon-lib/py/test/visionSystemSim_test.py Normal file
View File

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