Estimating Pose With PhotonVision

public class PhotonVision extends SubsystemBase {
    AprilTagFieldLayout aprilTagFieldLayout;

    PhotonCamera cam;
    Transform3d robotToCam;
    PhotonPoseEstimator poseEstimator;

    public PhotonVision(String camName, Transform3d robotToCam) {
        try {
            aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile);
        } catch (IOException e) {
            throw new RuntimeException(e);
        }

        cam = new PhotonCamera(camName);
        this.robotToCam = robotToCam;
        poseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.AVERAGE_BEST_TARGETS, cam, robotToCam);
    }

    public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
        return poseEstimator.update();
    }
}

public class ExampleSubsystem extends SubsystemBase {
    public void periodic() {
        if(!RobotState.isAutonomous()) {
            if(photonVision.getEstimatedGlobalPose().isPresent()) {
                EstimatedRobotPose estimatedPose = photonVision.getEstimatedGlobalPose().get();
                swerveDrive.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds, false, 4);
            }
        }
    }
}