From 9370937280df465e6cbabeba99dd5fa465e3635c Mon Sep 17 00:00:00 2001
From: amquake <noleetarrr@gmail.com>
Date: Sat, 4 Nov 2023 09:25:49 -0700
Subject: [PATCH] [photon-lib] Make PhotonPoseEstimator coprocessor multitag
 result relative to tag layout origin (#997)

Fixes #991.
---
 .../main/java/org/photonvision/PhotonPoseEstimator.java   | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
index 1c73f2cab2..32b4c7ba1f 100644
--- 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(