mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
e9006a2803
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Co-authored-by: Ryanforce08 <rradtke1208@gmail.com> Co-authored-by: PJ Reiniger <pj.reiniger@gmail.com> Co-authored-by: Jade Turner <spacey-sooty@proton.me> Co-authored-by: Matt Morley <matthew.morley.ca@gmail.com>
121 lines
4.2 KiB
Python
121 lines
4.2 KiB
Python
import math
|
|
from typing import overload
|
|
|
|
from wpimath import Pose3d, Translation3d
|
|
|
|
from ..estimation.targetModel import TargetModel
|
|
|
|
|
|
class VisionTargetSim:
|
|
"""Describes a vision target located somewhere on the field that your vision system can detect."""
|
|
|
|
@overload
|
|
def __init__(self, pose: Pose3d, model: TargetModel) -> None:
|
|
"""
|
|
Describes a retro-reflective/colored shape vision target located somewhere on the field that
|
|
your vision system can detect.
|
|
|
|
:param pose: Pose3d of the target in field-relative coordinates
|
|
:param model: TargetModel which describes the shape of the target
|
|
"""
|
|
...
|
|
|
|
@overload
|
|
def __init__(self, pose: Pose3d, model: TargetModel, id: int) -> None:
|
|
"""
|
|
Describes a fiducial tag located somewhere on the field that your vision system can detect.
|
|
|
|
:param pose: Pose3d of the tag in field-relative coordinates
|
|
:param model: TargetModel which describes the geometry of the target (tag)
|
|
:param id: The ID of this fiducial tag
|
|
"""
|
|
...
|
|
|
|
@overload
|
|
def __init__(
|
|
self, pose: Pose3d, model: TargetModel, objDetClassId: int, objDetConf: float
|
|
) -> None:
|
|
"""
|
|
Describes an object-detection vision target located somewhere on the field that your vision
|
|
system can detect.
|
|
|
|
:param pose: Pose3d of the target in field-relative coordinates
|
|
:param model: TargetModel which describes the shape of the target
|
|
:param objDetClassId: The object detection class ID, or -1 to exclude from object detection
|
|
:param objDetConf: The object detection confidence, or -1.0 to compute from target area
|
|
in the camera's field of view
|
|
"""
|
|
...
|
|
|
|
def __init__(
|
|
self,
|
|
pose: Pose3d,
|
|
model: TargetModel,
|
|
*args,
|
|
**kwargs,
|
|
):
|
|
if kwargs:
|
|
raise TypeError(
|
|
f"VisionTargetSim does not accept keyword arguments: {list(kwargs.keys())}"
|
|
)
|
|
|
|
self.pose: Pose3d = pose
|
|
self.model: TargetModel = model
|
|
|
|
if len(args) == 0:
|
|
# VisionTargetSim(pose, model)
|
|
self.fiducialId: int = -1
|
|
self.objDetClassId: int = -1
|
|
self.objDetConf: float = -1.0
|
|
elif len(args) == 1:
|
|
# VisionTargetSim(pose, model, id)
|
|
self.fiducialId = args[0]
|
|
self.objDetClassId = -1
|
|
self.objDetConf = -1.0
|
|
elif len(args) == 2:
|
|
# VisionTargetSim(pose, model, objDetClassId, objDetConf)
|
|
self.fiducialId = -1
|
|
self.objDetClassId = args[0]
|
|
self.objDetConf = args[1]
|
|
else:
|
|
raise ValueError(
|
|
f"VisionTargetSim takes 2-4 arguments, got {2 + len(args)}"
|
|
)
|
|
|
|
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]:
|
|
"""This target's vertices offset from its field pose."""
|
|
return self.model.getFieldVertices(self.pose)
|