mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
35 lines
1010 B
Python
35 lines
1010 B
Python
from dataclasses import dataclass, field
|
|
from wpimath.geometry import Transform3d
|
|
from ..packet import Packet
|
|
|
|
|
|
@dataclass
|
|
class PnpResult:
|
|
best: Transform3d = field(default_factory=Transform3d)
|
|
alt: Transform3d = field(default_factory=Transform3d)
|
|
ambiguity: float = 0.0
|
|
bestReprojError: float = 0.0
|
|
altReprojError: float = 0.0
|
|
|
|
photonStruct: "PNPResultSerde" = None
|
|
|
|
|
|
@dataclass
|
|
class MultiTargetPNPResult:
|
|
_MAX_IDS = 32
|
|
|
|
estimatedPose: PnpResult = field(default_factory=PnpResult)
|
|
fiducialIDsUsed: list[int] = field(default_factory=list)
|
|
|
|
def createFromPacket(self, packet: Packet) -> Packet:
|
|
self.estimatedPose = PnpResult()
|
|
self.estimatedPose.createFromPacket(packet)
|
|
self.fiducialIDsUsed = []
|
|
for _ in range(MultiTargetPNPResult._MAX_IDS):
|
|
fidId = packet.decode16()
|
|
if fidId >= 0:
|
|
self.fiducialIDsUsed.append(fidId)
|
|
return packet
|
|
|
|
photonStruct: "MultiTargetPNPResultSerde" = None
|