mirror of
https://github.com/Astatin3/photonvision-2025.0.0-beta-6.git
synced 2026-06-09 08:38:00 -06:00
26 lines
1.1 KiB
Python
26 lines
1.1 KiB
Python
|
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())
|
||
|
|
)
|