Merge pull request #186 from thedropbears/vision-cleanup

Small vision code cleanups/performance improvements

components/vision.py

@@ -7,6 +7,7 @@
 from magicbot import tunable
 from photonlibpy.photonCamera import PhotonCamera
 from photonlibpy.photonTrackedTarget import PhotonTrackedTarget
+from wpimath import objectToRobotPose
 from wpimath.geometry import Pose2d, Rotation3d, Transform3d, Translation3d, Pose3d
 
 from components.chassis import ChassisComponent
@@ -43,7 +44,8 @@ def __init__(
         chassis: ChassisComponent,
     ) -> None:
         self.camera = PhotonCamera(name)
-        self.camera_to_robot = Transform3d(pos, rot).inverse()
+        self.robot_to_camera = Transform3d(pos, rot)
+        self.camera_to_robot = self.robot_to_camera.inverse()
         self.last_timestamp = -1
         self.last_recieved_timestep = -1.0
 
@@ -92,26 +94,18 @@ def execute(self) -> None:
 
             if self.should_log:
                 self.multi_best_log.setPose(
-                    Pose2d(
-                        p.best.x,
-                        p.best.y,
-                        p.best.rotation().toRotation2d().radians(),
-                    )
+                    Pose2d(p.best.x, p.best.y, p.best.rotation().toRotation2d())
                 )
                 self.multi_alt_log.setPose(
-                    Pose2d(
-                        p.alt.x,
-                        p.alt.y,
-                        p.alt.rotation().toRotation2d().radians(),
-                    )
+                    Pose2d(p.alt.x, p.alt.y, p.alt.rotation().toRotation2d())
                 )
         else:
             for target in results.getTargets():
                 # filter out likely bad targets
                 if target.getPoseAmbiguity() > 0.25:
                     continue
 
-                poses = estimate_poses_from_apriltag(self.camera_to_robot, target)
+                poses = estimate_poses_from_apriltag(self.robot_to_camera, target)
                 if poses is None:
                     # tag doesn't exist
                     continue
@@ -131,18 +125,14 @@ def execute(self) -> None:
                         Pose2d(
                             target.bestCameraToTarget.x,
                             target.bestCameraToTarget.y,
-                            target.bestCameraToTarget.rotation()
-                            .toRotation2d()
-                            .radians(),
+                            target.bestCameraToTarget.rotation().toRotation2d(),
                         )
                     )
                     self.single_alt_log.setPose(
                         Pose2d(
                             target.altCameraToTarget.x,
                             target.altCameraToTarget.y,
-                            target.altCameraToTarget.rotation()
-                            .toRotation2d()
-                            .radians(),
+                            target.altCameraToTarget.rotation().toRotation2d(),
                         )
                     )
 
@@ -151,22 +141,20 @@ def sees_target(self):
 
 
 def estimate_poses_from_apriltag(
-    cam_to_robot: Transform3d, target: PhotonTrackedTarget
+    robot_to_camera: Transform3d, target: PhotonTrackedTarget
 ) -> Optional[tuple[Pose2d, Pose2d, float]]:
     tag_id = target.getFiducialId()
     tag_pose = apriltag_layout.getTagPose(tag_id)
     if tag_pose is None:
         return None
 
-    best_pose = tag_pose.transformBy(
-        target.getBestCameraToTarget().inverse()
-    ).transformBy(cam_to_robot)
-    alternate_pose = (
-        tag_pose.transformBy(target.getAlternateCameraToTarget().inverse())
-        .transformBy(cam_to_robot)
-        .toPose2d()
+    best_pose = objectToRobotPose(
+        tag_pose, target.getBestCameraToTarget(), robot_to_camera
+    )
+    alternate_pose = objectToRobotPose(
+        tag_pose, target.getAlternateCameraToTarget(), robot_to_camera
     )
-    return best_pose.toPose2d(), alternate_pose, best_pose.z
+    return best_pose.toPose2d(), alternate_pose.toPose2d(), best_pose.z
 
 
 def get_target_skew(target: PhotonTrackedTarget) -> float: