[photon-lib] Make PhotonPoseEstimator coprocessor multitag result rel…

…ative to tag layout origin (PhotonVision#997) Fixes PhotonVision#991.
mcm001 · Nov 4, 2023 · 9370937 · 9370937
1 parent 7eb4645
commit 9370937
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -100,7 +100,8 @@ public enum PoseStrategy {
      * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects
      *     with respect to the FIRST field using the <a href=
      *     "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
-     *     Coordinate System</a>.
+     *     Coordinate System</a>. Note that setting the origin of this layout object will affect the
+     *     results from this class.
      * @param strategy The strategy it should use to determine the best pose.
      * @param camera PhotonCamera
      * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
@@ -141,6 +142,8 @@ private void checkUpdate(Object oldObj, Object newObj) {
     /**
      * Get the AprilTagFieldLayout being used by the PositionEstimator.
      *
+     * <p>Note: Setting the origin of this layout will affect the results from this class.
+     *
      * @return the AprilTagFieldLayout
      */
     public AprilTagFieldLayout getFieldTags() {
@@ -150,6 +153,8 @@ public AprilTagFieldLayout getFieldTags() {
     /**
      * Set the AprilTagFieldLayout being used by the PositionEstimator.
      *
+     * <p>Note: Setting the origin of this layout will affect the results from this class.
+     *
      * @param fieldTags the AprilTagFieldLayout
      */
     public void setFieldTags(AprilTagFieldLayout fieldTags) {
@@ -415,6 +420,7 @@ private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
             var best =
                     new Pose3d()
                             .plus(best_tf) // field-to-camera
+                            .relativeTo(fieldTags.getOrigin())
                             .plus(robotToCamera.inverse()); // field-to-robot
             return Optional.of(
                     new EstimatedRobotPose(