Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-27 02:01: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/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)