Change pose estimator to take robotToCamera (#698)

Co-authored-by: Matt <matthew.morley.ca@gmail.com>

photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java

@@ -73,14 +73,11 @@ public class RobotPoseEstimator {
     /**
      * Create a new RobotPoseEstimator.
      *
-     * <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>(); <p> map.put(1, new
-     * Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is at (1.0,2.0,3.0) </code> }
-     *
-     * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with respect to the
-     *     FIRST field.
+     * @param aprilTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3ds with
+     *     respect to the FIRST field.
      * @param strategy The strategy it should use to determine the best pose.
      * @param cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from
-     *     the center of the robot to the cameras.
+     *     the center of the robot to the camera mount positions (ie, robot -> camera).
      */
     public RobotPoseEstimator(
             AprilTagFieldLayout aprilTags,
@@ -96,7 +93,7 @@ public class RobotPoseEstimator {
     /**
      * Update the estimated pose using the selected strategy.
      *
-     * @return The updated estimated pose and the latency in milliseconds Estimated pose may be null
+     * @return The updated estimated pose and the latency in milliseconds. Estimated pose may be null
      *     if no targets were seen
      */
     public Optional<Pair<Pose3d, Double>> update() {

photon-lib/src/main/java/org/photonvision/SimVisionSystem.java

@@ -24,6 +24,8 @@
 
 package org.photonvision;
 
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -49,7 +51,7 @@ public class SimVisionSystem {
     int cameraResWidth;
     int cameraResHeight;
     double minTargetArea;
-    Transform3d cameraToRobot;
+    Transform3d robotToCamera;
 
     Field2d dbgField;
     FieldObject2d dbgRobot;
@@ -67,7 +69,8 @@ public class SimVisionSystem {
      * @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the
      *     manufacturer specifications, and/or whatever is configured in the PhotonVision Setting
      *     page.
-     * @param cameraToRobot Transform to move from the camera's mount position to the robot's position
+     * @param robotToCamera Transform to move from the center of the robot to the camera's mount
+     *     position
      * @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and
      *     make it visible. Set to 9000 or more if your vision system does not rely on LED's.
      * @param cameraResWidth Width of your camera's image sensor in pixels
@@ -79,12 +82,12 @@ public class SimVisionSystem {
     public SimVisionSystem(
             String camName,
             double camDiagFOVDegrees,
-            Transform3d cameraToRobot,
+            Transform3d robotToCamera,
             double maxLEDRangeMeters,
             int cameraResWidth,
             int cameraResHeight,
             double minTargetArea) {
-        this.cameraToRobot = cameraToRobot;
+        this.robotToCamera = robotToCamera;
         this.maxLEDRangeMeters = maxLEDRangeMeters;
         this.cameraResWidth = cameraResWidth;
         this.cameraResHeight = cameraResHeight;
@@ -119,14 +122,31 @@ public class SimVisionSystem {
         ;
     }
 
+    /**
+     * Adds all apriltags from the provided {@link AprilTagFieldLayout} as sim vision targets. The
+     * poses added will preserve the tag layout's alliance origin at the time of calling this method.
+     *
+     * @param tagLayout The field tag layout to get Apriltag poses and IDs from
+     */
+    public void addVisionTargets(AprilTagFieldLayout tagLayout) {
+        for (AprilTag tag : tagLayout.getTags()) {
+            addSimVisionTarget(
+                    new SimVisionTarget(
+                            tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
+                            Units.inchesToMeters(6),
+                            Units.inchesToMeters(6),
+                            tag.ID));
+        }
+    }
+
     /**
      * Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or
      * turret or some other mobile platform.
      *
-     * @param newCameraToRobot New Transform from the robot to the camera
+     * @param newRobotToCamera New Transform from the robot to the camera
      */
-    public void moveCamera(Transform3d newCameraToRobot) {
-        this.cameraToRobot = newCameraToRobot;
+    public void moveCamera(Transform3d newRobotToCamera) {
+        this.robotToCamera = newRobotToCamera;
     }
 
     /**
@@ -150,7 +170,7 @@ public class SimVisionSystem {
      *     PhotonVision parameters.
      */
     public void processFrame(Pose3d robotPoseMeters) {
-        Pose3d cameraPose = robotPoseMeters.transformBy(cameraToRobot.inverse());
+        Pose3d cameraPose = robotPoseMeters.transformBy(robotToCamera);
 
         dbgRobot.setPose(robotPoseMeters.toPose2d());
         dbgCamera.setPose(cameraPose.toPose2d());