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());

photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h

@@ -56,10 +56,25 @@ class RobotPoseEstimator {
       std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
   using size_type = std::vector<map_value_type>::size_type;
 
+  /**
+   * Create a new RobotPoseEstimator.
+   *
+   * @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 camera mount positions
+   * (ie, robot -> camera).
+   */
   explicit RobotPoseEstimator(
       std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
       PoseStrategy strategy, std::vector<map_value_type> cameras);
 
+  /**
+   * Update the estimated pose using the selected strategy.
+   *
+   * @return The updated estimated pose and the latency in milliseconds.
+   */
   std::pair<frc::Pose3d, units::millisecond_t> Update();
 
   inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }

photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java

@@ -182,8 +182,7 @@ class SimVisionSystemTest {
                 new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI));
         var robotToCamera =
                 new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, Math.PI / 4, 0));
-        var sysUnderTest =
-                new SimVisionSystem("Test", 80.0, robotToCamera.inverse(), 99999, 1234, 1234, 0);
+        var sysUnderTest = new SimVisionSystem("Test", 80.0, robotToCamera, 99999, 1234, 1234, 0);
         sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 1736));
 
         var robotPose = new Pose2d(new Translation2d(14.98, 0), Rotation2d.fromDegrees(5));
@@ -247,14 +246,15 @@ class SimVisionSystemTest {
     @ParameterizedTest
     @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999})
     public void testCameraPitch(double testPitch) {
+        testPitch = testPitch * -1;
+
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4));
         final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0));
         var sysUnderTest = new SimVisionSystem("Test", 120.0, new Transform3d(), 99999, 640, 480, 0);
         sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 23));
 
-        // Here, passing in a positive testPitch points the camera downward (since moveCamera takes the
-        // camera->robot transform)
+        // Transform is now robot -> camera
         sysUnderTest.moveCamera(
                 new Transform3d(
                         new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0)));
@@ -263,10 +263,11 @@ class SimVisionSystemTest {
         assertTrue(res.hasTargets());
         var tgt = res.getBestTarget();
 
-        // Since the camera is level with the target, a downward point will mean the target is in the
-        // upper half of the image
-        // which should produce positive pitch.
-        assertEquals(testPitch, tgt.getPitch(), 0.0001);
+        // Since the camera is level with the target, a positive-upward point will mean the target is in
+        // the
+        // lower half of the image
+        // which should produce negative pitch.
+        assertEquals(testPitch * -1, tgt.getPitch(), 0.0001);
     }
 
     private static Stream<Arguments> distCalCParamProvider() {
@@ -309,7 +310,7 @@ class SimVisionSystemTest {
                 new SimVisionSystem(
                         "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!",
                         160.0,
-                        robotToCamera.inverse(),
+                        robotToCamera,
                         99999,
                         640,
                         480,