diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 1c73f2cab..32b4c7ba1 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 class PhotonPoseEstimator { * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects * with respect to the FIRST field using the Field - * Coordinate System. + * Coordinate System. 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 @@ public class PhotonPoseEstimator { /** * Get the AprilTagFieldLayout being used by the PositionEstimator. * + *

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 class PhotonPoseEstimator { /** * Set the AprilTagFieldLayout being used by the PositionEstimator. * + *

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 @@ public class PhotonPoseEstimator { var best = new Pose3d() .plus(best_tf) // field-to-camera + .relativeTo(fieldTags.getOrigin()) .plus(robotToCamera.inverse()); // field-to-robot return Optional.of( new EstimatedRobotPose(