Bump LL image to fix NetworkManager (#780 )

Fix desync between web UI and NT (#778 )
Actually calls VisionModule::setPipeline when changing pipelines (needed to change video modes)
2026-06-19 00:41:41 +00:00 · 2023-01-31 06:57:45 -06:00 · 2023-01-29 23:30:34 -05:00 · 2023-01-29 23:30:22 -05:00 · 2023-01-27 10:52:14 -05:00 · 2023-01-22 10:56:41 -05:00
63 changed files with 2160 additions and 457 deletions
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -393,11 +393,11 @@ jobs:
            - os: ubuntu-latest
              artifact-name: LinuxArm64
              image_suffix: RaspberryPi
-              image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.1.0-beta-4_arm64
+              image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.1.1_arm64
            - os: ubuntu-latest
              artifact-name: LinuxArm64
              image_suffix: limelight
-              image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.1.0-beta-5_limelight-arm64
+              image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.2.2_limelight-arm64

    runs-on: ${{ matrix.os }}
    name: "Build image - ${{ matrix.image_url }}"
--- a/build.gradle
+++ b/build.gradle
@@ -26,7 +26,7 @@ allprojects {
 apply from: "versioningHelper.gradle"

 ext {
-    wpilibVersion = "2023.1.1"
+    wpilibVersion = "2023.2.1"
    opencvVersion = "4.6.0-4"
    joglVersion = "2.4.0-rc-20200307"
    pubVersion = versionString
--- a/photon-client/src/views/CamerasView.vue
+++ b/photon-client/src/views/CamerasView.vue
@@ -72,6 +72,14 @@
                    :disabled="isCalibrating"
                    tooltip="Resolution to calibrate at (you will have to calibrate every resolution you use 3D mode on)"
                  />
+                  <CVselect
+                    v-model="streamingFrameDivisor"
+                    name="Decimation"
+                    tooltip="Resolution to which camera frames are downscaled for detection. Calibration still uses full-res"
+                    :list="calibrationDivisors"
+                    select-cols="7"
+                    @rollback="e => rollback('streamingFrameDivisor', e)"
+                  />
                  <CVselect
                    v-model="boardType"
                    name="Board Type"
@@ -291,6 +299,19 @@
                  Download Target
                </v-btn>
              </v-col>
+              <v-col>
+                <v-btn
+                  color="secondary"
+                  small
+                  style="width: 100%;"
+                  @click="$refs.importCalibrationFromCalibdb.click()"
+                >
+                  <v-icon left>
+                    mdi-upload
+                  </v-icon>
+                  Import From CalibDB
+                </v-btn>
+              </v-col>
            </v-row>
          </div>
        </v-card>
@@ -367,6 +388,20 @@
        </template>
      </v-col>
    </v-row>
+
+    <!-- Special hidden upload input that gets 'clicked' when the user imports settings -->
+    <input
+      ref="importCalibrationFromCalibdb"
+      type="file"
+      accept=".json"
+      style="display: none;"
+      @change="readImportedCalibration"
+    />
+
+    <v-snackbar v-model="uploadSnack" top :color="uploadSnackData.color" timeout="-1">
+      <span>{{ uploadSnackData.text }}</span>
+    </v-snackbar>
+
  </div>
 </template>

@@ -397,6 +432,12 @@ export default {
            calibrationFailed: false,
            filteredVideomodeIndex: 0,
            settingsValid: true,
+            unfilteredStreamDivisors: [1, 2, 4],
+            uploadSnackData: {
+              color: "success",
+              text: "",
+            },
+            uploadSnack: false,
        }
    },
    computed: {
@@ -430,6 +471,22 @@ export default {
          }
        },

+        calibrationDivisors: {
+          get() {
+            return this.unfilteredStreamDivisors.filter(item => {
+              var res = this.stringResolutionList[this.selectedFilteredResIndex].split(" X ").map(it => parseInt(it));
+              console.log(res);
+              console.log(item);
+              // Realistically, we need more than 320x240, but lower than this is
+              // basically unusable. For now, don't allow decimations that take us
+              // below that
+              const ret = ((res[0] / item) >= 300 && (res[1] / item) >= 220) || (item === 1);
+              console.log(ret);
+              return ret;
+            })
+          }
+        },
+
        // Makes sure there's only one entry per resolution
        filteredResolutionList: {
            get() {
@@ -466,6 +523,17 @@ export default {
                this.$store.commit('cameraSettings', value);
            }
        },
+
+        streamingFrameDivisor: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.streamingFrameDivisor;
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"streamingFrameDivisor": val});
+                this.handlePipelineUpdate("streamingFrameDivisor", val);
+            }
+        },
+
        boardType: {
            get() {
                return this.calibrationData.boardType
@@ -535,6 +603,57 @@ export default {
        },
    },
    methods: {
+
+    readImportedCalibration(event) {
+      // let formData = new FormData();
+      // formData.append("zipData", event.target.files[0]);
+      const filename = event.target.files[0].name;
+
+      event.target.files[0].text().then(fileText => {
+        const data = {
+          "cameraIndex": this.$store.getters.currentCameraIndex,
+          "payload": fileText,
+          "filename": filename,
+        };
+
+        this.axios
+          .post("http://" + this.$address + "/api/calibration/import", data, {
+            headers: { "Content-Type": "text/plain" },
+          })
+          .then(() => {
+            this.uploadSnackData = {
+              color: "success",
+              text:
+                "Calibration imported successfully! PhotonVision will restart in the background...",
+            };
+            this.uploadSnack = true;
+          })
+          .catch((err) => {
+            if (err.response) {
+              this.uploadSnackData = {
+                color: "error",
+                text:
+                  "Error while uploading calibration file! Could not process provided file.",
+              };
+            } else if (err.request) {
+              this.uploadSnackData = {
+                color: "error",
+                text:
+                  "Error while uploading calibration file! No respond to upload attempt.",
+              };
+            } else {
+              this.uploadSnackData = {
+                color: "error",
+                text: "Error while uploading calibration file!",
+              };
+            }
+            this.uploadSnack = true;
+          });
+
+        })
+
+    },
+
        closeDialog() {
            this.snack = false;
            this.calibrationInProgress = false;
--- a/photon-client/src/views/SettingsViews/Networking.vue
+++ b/photon-client/src/views/SettingsViews/Networking.vue
@@ -21,7 +21,7 @@
        Team number is unset or invalid. NetworkTables will not be able to connect.
      </v-banner>
      <CVradio
-        v-show="$store.state.settings.networkSettings.supported"
+        v-show="$store.state.settings.networkSettings.shouldManage"
        v-model="connectionType"
        :input-cols="inputCols"
        name="IP Assignment Mode"
@@ -74,7 +74,33 @@
    >
      <span>{{ snackbar.text }}</span>
    </v-snackbar>
-    <v-divider class="mt-4 mb-4" />
+
+    <template v-if="$store.state.settings.networkSettings.shouldManage && false">
+
+      <!-- Advanced controls for changing DHCP settings and stuff -->
+      <v-divider class="mt-4 mb-4" />
+
+      <v-title> Advanced </v-title>
+
+      <CVinput
+        :input-cols="inputCols"
+        name="Set DHCP command"
+      />
+      <CVinput
+        :input-cols="inputCols"
+        name="Set static command"
+      />
+      <CVinput
+        :input-cols="inputCols"
+        name="NetworkManager interface"
+      />
+      <CVinput
+        :input-cols="inputCols"
+        name="Physical interface"
+      />
+
+    </template>
+
    <!-- TEMP - RIO finder is not currently enabled
    <v-row>
      <v-col
@@ -246,6 +272,14 @@ export default {
            return true;
        },
        sendGeneralSettings() {
+            const changingStaticIp = !this.isDHCP;
+
+            this.snackbar = {
+                color: "secondary",
+                text: "Updating settings..."
+            };
+            this.snack = true;
+
            this.axios.post("http://" + this.$address + "/api/settings/general", this.settings).then(
                response => {
                    if (response.status === 200) {
@@ -257,10 +291,17 @@ export default {
                    }
                },
                error => {
+                  if (error.status === 504 || changingStaticIp) {
+                    this.snackbar = {
+                        color: "error",
+                        text: (error.response || {data: `Connection lost! Try the new static IP at ${this.staticIp}:5800 or ${this.hostname}:5800 ?`}).data
+                    };
+                  } else {
                    this.snackbar = {
                        color: "error",
                        text: (error.response || {data: "Couldn't save settings"}).data
                    };
+                  }
                    this.snack = true;
                }
            )
--- a/photon-core/src/main/java/org/photonvision/common/configuration/NetworkConfig.java
+++ b/photon-core/src/main/java/org/photonvision/common/configuration/NetworkConfig.java
@@ -19,12 +19,15 @@ package org.photonvision.common.configuration;

 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonGetter;
+import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import com.fasterxml.jackson.annotation.JsonSetter;
+import com.fasterxml.jackson.databind.ObjectMapper;
 import java.util.HashMap;
 import java.util.Map;
 import org.photonvision.common.hardware.Platform;
 import org.photonvision.common.networking.NetworkMode;
+import org.photonvision.common.util.file.JacksonUtils;

 public class NetworkConfig {
    public int teamNumber = 0;
@@ -33,6 +36,16 @@ public class NetworkConfig {
    public String hostname = "photonvision";
    public boolean runNTServer = false;

+    @JsonIgnore public static final String NM_IFACE_STRING = "${interface}";
+    @JsonIgnore public static final String NM_IP_STRING = "${ipaddr}";
+
+    public String networkManagerIface = "Wired\\ connection\\ 1";
+    public String physicalInterface = "eth0";
+    public String setStaticCommand =
+            "nmcli con mod ${interface} ipv4.addresses ${ipaddr}/8 ipv4.method \"manual\" ipv6.method \"disabled\"";
+    public String setDHCPcommand =
+            "nmcli con mod ${interface} ipv4.method \"auto\" ipv6.method \"disabled\"";
+
    private boolean shouldManage;

    public NetworkConfig() {
@@ -46,37 +59,39 @@ public class NetworkConfig {
            @JsonProperty("staticIp") String staticIp,
            @JsonProperty("hostname") String hostname,
            @JsonProperty("runNTServer") boolean runNTServer,
-            @JsonProperty("shouldManage") boolean shouldManage) {
+            @JsonProperty("shouldManage") boolean shouldManage,
+            @JsonProperty("networkManagerIface") String networkManagerIface,
+            @JsonProperty("physicalInterface") String physicalInterface,
+            @JsonProperty("setStaticCommand") String setStaticCommand,
+            @JsonProperty("setDHCPcommand") String setDHCPcommand) {
        this.teamNumber = teamNumber;
        this.connectionType = connectionType;
        this.staticIp = staticIp;
        this.hostname = hostname;
        this.runNTServer = runNTServer;
+        this.networkManagerIface = networkManagerIface;
+        this.physicalInterface = physicalInterface;
+        this.setStaticCommand = setStaticCommand;
+        this.setDHCPcommand = setDHCPcommand;
        setShouldManage(shouldManage);
    }

    public static NetworkConfig fromHashMap(Map<String, Object> map) {
-        // teamNumber (int), supported (bool), connectionType (int),
-        // staticIp (str), netmask (str), hostname (str)
-        var ret = new NetworkConfig();
-        ret.teamNumber = Integer.parseInt(map.get("teamNumber").toString());
-        ret.connectionType = NetworkMode.values()[(Integer) map.get("connectionType")];
-        ret.staticIp = (String) map.get("staticIp");
-        ret.hostname = (String) map.get("hostname");
-        ret.runNTServer = (Boolean) map.get("runNTServer");
-        ret.setShouldManage((Boolean) map.get("supported"));
-        return ret;
+        try {
+            return new ObjectMapper().convertValue(map, NetworkConfig.class);
+        } catch (Exception e) {
+            e.printStackTrace();
+            return new NetworkConfig();
+        }
    }

-    public HashMap<String, Object> toHashMap() {
-        HashMap<String, Object> tmp = new HashMap<>();
-        tmp.put("teamNumber", teamNumber);
-        tmp.put("supported", shouldManage());
-        tmp.put("connectionType", connectionType.ordinal());
-        tmp.put("staticIp", staticIp);
-        tmp.put("hostname", hostname);
-        tmp.put("runNTServer", runNTServer);
-        return tmp;
+    public Map<String, Object> toHashMap() {
+        try {
+            return new ObjectMapper().convertValue(this, JacksonUtils.UIMap.class);
+        } catch (Exception e) {
+            e.printStackTrace();
+            return new HashMap<>();
+        }
    }

    @JsonGetter("shouldManage")
--- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
+++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
@@ -142,8 +142,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer {

        ts.rawBytesEntry.set(packet.getData());

-        ts.pipelineIndexPublisher.setDefault(pipelineIndexSupplier.get());
-        ts.driverModePublisher.setDefault(driverModeSupplier.getAsBoolean());
+        ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
+        ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());
        ts.latencyMillisEntry.set(result.getLatencyMillis());
        ts.hasTargetEntry.set(result.hasTargets());

--- a/photon-core/src/main/java/org/photonvision/common/networking/NetworkManager.java
+++ b/photon-core/src/main/java/org/photonvision/common/networking/NetworkManager.java
@@ -18,6 +18,7 @@
 package org.photonvision.common.networking;

 import org.photonvision.common.configuration.ConfigManager;
+import org.photonvision.common.configuration.NetworkConfig;
 import org.photonvision.common.hardware.Platform;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
@@ -48,8 +49,7 @@ public class NetworkManager {
        logger.info("Setting " + config.connectionType + " with team team " + config.teamNumber);
        if (Platform.isLinux()) {
            if (!Platform.isRoot()) {
-                logger.error("Cannot manage network without root!");
-                return;
+                logger.error("Cannot manage hostname without root!");
            }

            // always set hostname
@@ -96,10 +96,11 @@ public class NetworkManager {
            if (config.connectionType == NetworkMode.DHCP) {
                var shell = new ShellExec();
                try {
-                    if (!config.staticIp.equals("")) {
-                        shell.executeBashCommand("ip addr del " + config.staticIp + "/8 dev eth0");
-                    }
-                    shell.executeBashCommand("dhclient eth0", false);
+                    // set nmcli back to DHCP, and re-run dhclient -- this ought to grab a new IP address
+                    shell.executeBashCommand(
+                            config.setDHCPcommand.replace(
+                                    NetworkConfig.NM_IFACE_STRING, config.networkManagerIface));
+                    shell.executeBashCommand("dhclient " + config.physicalInterface, false);
                } catch (Exception e) {
                    logger.error("Exception while setting DHCP!");
                }
@@ -107,7 +108,30 @@ public class NetworkManager {
                var shell = new ShellExec();
                if (config.staticIp.length() > 0) {
                    try {
-                        shell.executeBashCommand("ip addr add " + config.staticIp + "/8" + " dev eth0");
+                        shell.executeBashCommand(
+                                config
+                                        .setStaticCommand
+                                        .replace(NetworkConfig.NM_IFACE_STRING, config.networkManagerIface)
+                                        .replace(NetworkConfig.NM_IP_STRING, config.staticIp));
+
+                        if (Platform.isRaspberryPi()) {
+                            // Pi's need to manually have their interface adjusted?? and the 5 second sleep is
+                            // integral in my testing (Matt)
+                            shell.executeBashCommand(
+                                    "sh -c 'nmcli con down "
+                                            + config.networkManagerIface
+                                            + "; nmcli con up "
+                                            + config.networkManagerIface
+                                            + "'");
+                        } else {
+                            // for now just bring down /up -- more testing needed on beelink et al
+                            shell.executeBashCommand(
+                                    "sh -c 'nmcli con down "
+                                            + config.networkManagerIface
+                                            + "; nmcli con up "
+                                            + config.networkManagerIface
+                                            + "'");
+                        }
                    } catch (Exception e) {
                        logger.error("Error while setting static IP!", e);
                    }
--- a/photon-core/src/main/java/org/photonvision/common/networking/NetworkMode.java
+++ b/photon-core/src/main/java/org/photonvision/common/networking/NetworkMode.java
@@ -17,7 +17,14 @@

 package org.photonvision.common.networking;

+import com.fasterxml.jackson.annotation.JsonValue;
+
 public enum NetworkMode {
    DHCP,
-    STATIC
+    STATIC;
+
+    @JsonValue
+    public int toValue() {
+        return ordinal();
+    }
 }
--- a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
+++ b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
@@ -22,6 +22,7 @@ import edu.wpi.first.apriltag.jni.AprilTagJNI;
 import edu.wpi.first.cscore.CameraServerCvJNI;
 import edu.wpi.first.cscore.CameraServerJNI;
 import edu.wpi.first.hal.JNIWrapper;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.net.WPINetJNI;
 import edu.wpi.first.networktables.NetworkTablesJNI;
@@ -139,6 +140,34 @@ public class TestUtils {
        }
    }

+    public enum WPI2023Apriltags {
+        k162_36_Angle,
+        k162_36_Straight,
+        k383_60_Angle2;
+
+        public static double FOV = 68.5;
+
+        public final Translation2d approxPose;
+        public final Path path;
+
+        Path getPath() {
+            var filename = this.toString().substring(1);
+            return Path.of("2023", "AprilTags", filename + ".png");
+        }
+
+        Translation2d getPose() {
+            var names = this.toString().substring(1).split("_");
+            var x = Units.inchesToMeters(Integer.parseInt(names[0]));
+            var y = Units.inchesToMeters(Integer.parseInt(names[1]));
+            return new Translation2d(x, y);
+        }
+
+        WPI2023Apriltags() {
+            this.approxPose = getPose();
+            this.path = getPath();
+        }
+    }
+
    public enum WPI2022Image {
        kTerminal12ft6in(Units.feetToMeters(12.5)),
        kTerminal22ft6in(Units.feetToMeters(22.5));
@@ -215,7 +244,7 @@ public class TestUtils {
        }
    }

-    private static Path getResourcesFolderPath(boolean testMode) {
+    public static Path getResourcesFolderPath(boolean testMode) {
        System.out.println("CWD: " + Path.of("").toAbsolutePath().toString());

        // VSCode likes to make this path relative to the wrong root directory, so a fun hack to tell
@@ -306,6 +335,7 @@ public class TestUtils {

    private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json";
    private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json";
+    public static final String LIFECAM_1280P_CAL_FILE = "lifecam_1280.json";
    public static final String LIMELIGHT_480P_CAL_FILE = "limelight_1280_720.json";

    public static CameraCalibrationCoefficients getCoeffs(String filename, boolean testMode) {
@@ -355,4 +385,14 @@ public class TestUtils {
    public static void showImage(Mat frame) {
        showImage(frame, DefaultTimeoutMillis);
    }
+
+    public static Path getTestMode2023ImagePath() {
+        return getResourcesFolderPath(true)
+                .resolve("testimages")
+                .resolve(WPI2022Image.kTerminal22ft6in.path);
+    }
+
+    public static CameraCalibrationCoefficients get2023LifeCamCoeffs(boolean testMode) {
+        return getCoeffs(LIFECAM_1280P_CAL_FILE, testMode);
+    }
 }
--- a/photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java
+++ b/photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java
@@ -31,8 +31,11 @@ import java.io.FileDescriptor;
 import java.io.FileOutputStream;
 import java.io.IOException;
 import java.nio.file.Path;
+import java.util.HashMap;

 public class JacksonUtils {
+    public static class UIMap extends HashMap<String, Object> {}
+
    public static <T> void serialize(Path path, T object) throws IOException {
        serialize(path, object, true);
    }
--- a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java
+++ b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java
@@ -21,6 +21,7 @@ import com.fasterxml.jackson.annotation.JsonAlias;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.databind.JsonNode;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfDouble;
 import org.opencv.core.Size;
@@ -82,4 +83,43 @@ public class CameraCalibrationCoefficients implements Releasable {
        cameraIntrinsics.release();
        distCoeffs.release();
    }
+
+    public static CameraCalibrationCoefficients parseFromCalibdbJson(JsonNode json) {
+        // camera_matrix is a row major, array of arrays
+        var cam_matrix = json.get("camera_matrix");
+
+        double[] cam_arr =
+                new double[] {
+                    cam_matrix.get(0).get(0).doubleValue(),
+                    cam_matrix.get(0).get(1).doubleValue(),
+                    cam_matrix.get(0).get(2).doubleValue(),
+                    cam_matrix.get(1).get(0).doubleValue(),
+                    cam_matrix.get(1).get(1).doubleValue(),
+                    cam_matrix.get(1).get(2).doubleValue(),
+                    cam_matrix.get(2).get(0).doubleValue(),
+                    cam_matrix.get(2).get(1).doubleValue(),
+                    cam_matrix.get(2).get(2).doubleValue()
+                };
+
+        var dist_coefs = json.get("distortion_coefficients");
+
+        double[] dist_array =
+                new double[] {
+                    dist_coefs.get(0).doubleValue(),
+                    dist_coefs.get(1).doubleValue(),
+                    dist_coefs.get(2).doubleValue(),
+                    dist_coefs.get(3).doubleValue(),
+                    dist_coefs.get(4).doubleValue(),
+                };
+
+        var cam_jsonmat = new JsonMat(3, 3, cam_arr);
+        var distortion_jsonmat = new JsonMat(1, 5, dist_array);
+
+        var error = json.get("avg_reprojection_error").asDouble();
+        var width = json.get("img_size").get(0).doubleValue();
+        var height = json.get("img_size").get(1).doubleValue();
+
+        return new CameraCalibrationCoefficients(
+                new Size(width, height), cam_jsonmat, distortion_jsonmat, new double[] {error}, 0);
+    }
 }
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
@@ -17,7 +17,6 @@

 package org.photonvision.vision.pipe.impl;

-import java.util.Objects;
 import org.apache.commons.lang3.tuple.Pair;
 import org.apache.commons.lang3.tuple.Triple;
 import org.opencv.calib3d.Calib3d;
@@ -25,6 +24,7 @@ import org.opencv.core.*;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.photonvision.vision.frame.FrameDivisor;
 import org.photonvision.vision.pipe.CVPipe;
 import org.photonvision.vision.pipeline.UICalibrationData;

@@ -39,10 +39,6 @@ public class FindBoardCornersPipe
    Size imageSize;
    Size patternSize;

-    // Tune to taste for a reasonable tradeoff between making
-    // the findCorners portion work hard, versus the subpixel refinement work hard.
-    final int FIND_CORNERS_WIDTH_PX = 320;
-
    // Configure the optimizations used while using openCV's find corners algorithm
    // Since we return results in real-time, we want ensure it goes as fast as possible
    // and fails as fast as possible.
@@ -125,17 +121,13 @@ public class FindBoardCornersPipe

    /**
     * Figures out how much a frame or point cloud must be scaled down by to match the desired size at
-     * which to run FindCorners
+     * which to run FindCorners. Should usually be > 1.
     *
     * @param inFrame
     * @return
     */
    private double getFindCornersScaleFactor(Mat inFrame) {
-        if (inFrame.width() > FIND_CORNERS_WIDTH_PX) {
-            return ((double) FIND_CORNERS_WIDTH_PX) / inFrame.width();
-        } else {
-            return 1.0;
-        }
+        return 1.0 / params.divisor.value;
    }

    /**
@@ -174,9 +166,10 @@ public class FindBoardCornersPipe
     *     findBoardCorners
     * @return the size to scale the input mat to
     */
-    private Size getFindCornersImgSize(Mat inFrame) {
-        var findcorners_height = Math.round(inFrame.height() * getFindCornersScaleFactor(inFrame));
-        return new Size(FIND_CORNERS_WIDTH_PX, findcorners_height);
+    private Size getFindCornersImgSize(Mat in) {
+        int width = in.cols() / params.divisor.value;
+        int height = in.rows() / params.divisor.value;
+        return new Size(width, height);
    }

    /**
@@ -295,29 +288,48 @@ public class FindBoardCornersPipe
        private final int boardWidth;
        private final UICalibrationData.BoardType type;
        private final double gridSize;
+        private final FrameDivisor divisor;

        public FindCornersPipeParams(
-                int boardHeight, int boardWidth, UICalibrationData.BoardType type, double gridSize) {
+                int boardHeight,
+                int boardWidth,
+                UICalibrationData.BoardType type,
+                double gridSize,
+                FrameDivisor divisor) {
            this.boardHeight = boardHeight;
            this.boardWidth = boardWidth;
            this.type = type;
            this.gridSize = gridSize; // mm
-        }
-
-        @Override
-        public boolean equals(Object o) {
-            if (this == o) return true;
-            if (o == null || getClass() != o.getClass()) return false;
-            FindCornersPipeParams that = (FindCornersPipeParams) o;
-            return boardHeight == that.boardHeight
-                    && boardWidth == that.boardWidth
-                    && Double.compare(that.gridSize, gridSize) == 0
-                    && type == that.type;
+            this.divisor = divisor;
        }

        @Override
        public int hashCode() {
-            return Objects.hash(boardHeight, boardWidth, type, gridSize);
+            final int prime = 31;
+            int result = 1;
+            result = prime * result + boardHeight;
+            result = prime * result + boardWidth;
+            result = prime * result + ((type == null) ? 0 : type.hashCode());
+            long temp;
+            temp = Double.doubleToLongBits(gridSize);
+            result = prime * result + (int) (temp ^ (temp >>> 32));
+            result = prime * result + ((divisor == null) ? 0 : divisor.hashCode());
+            return result;
+        }
+
+        @Override
+        public boolean equals(Object obj) {
+            if (this == obj) return true;
+            if (obj == null) return false;
+            if (getClass() != obj.getClass()) return false;
+            FindCornersPipeParams other = (FindCornersPipeParams) obj;
+            if (boardHeight != other.boardHeight) return false;
+            if (boardWidth != other.boardWidth) return false;
+            if (type != other.type) return false;
+            if (Double.doubleToLongBits(gridSize) != Double.doubleToLongBits(other.gridSize))
+                return false;
+            if (divisor != other.divisor) return false;
+            return true;
        }
    }
 }
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
@@ -30,7 +30,7 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
    public int threads = 4; // Multiple threads seems to be better performance on most platforms
    public boolean debug = false;
    public boolean refineEdges = true;
-    public int numIterations = 200;
+    public int numIterations = 40;
    public int hammingDist = 0;
    public int decisionMargin = 35;

--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
@@ -87,7 +87,11 @@ public class Calibrate3dPipeline
    protected void setPipeParamsImpl() {
        FindBoardCornersPipe.FindCornersPipeParams findCornersPipeParams =
                new FindBoardCornersPipe.FindCornersPipeParams(
-                        settings.boardHeight, settings.boardWidth, settings.boardType, settings.gridSize);
+                        settings.boardHeight,
+                        settings.boardWidth,
+                        settings.boardType,
+                        settings.gridSize,
+                        settings.streamingFrameDivisor);
        findBoardCornersPipe.setParams(findCornersPipeParams);

        Calibrate3dPipe.CalibratePipeParams calibratePipeParams =
@@ -105,7 +109,7 @@ public class Calibrate3dPipeline
    protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings settings) {
        Mat inputColorMat = frame.colorImage.getMat();

-        if (this.calibrating) {
+        if (this.calibrating || inputColorMat.empty()) {
            return new CVPipelineResult(0, 0, null, frame);
        }

--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java
@@ -19,6 +19,7 @@ package org.photonvision.vision.pipeline;

 import edu.wpi.first.math.util.Units;
 import org.opencv.core.Size;
+import org.photonvision.vision.frame.FrameDivisor;

 public class Calibration3dPipelineSettings extends AdvancedPipelineSettings {
    public int boardHeight = 8;
@@ -33,5 +34,6 @@ public class Calibration3dPipelineSettings extends AdvancedPipelineSettings {
        this.cameraAutoExposure = true;
        this.inputShouldShow = true;
        this.outputShouldShow = true;
+        this.streamingFrameDivisor = FrameDivisor.HALF;
    }
 }
--- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
+++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
@@ -136,7 +136,7 @@ public class VisionModule {
                new NTDataPublisher(
                        visionSource.getSettables().getConfiguration().nickname,
                        pipelineManager::getCurrentPipelineIndex,
-                        pipelineManager::setIndex,
+                        this::setPipeline,
                        pipelineManager::getDriverMode,
                        this::setDriverMode);
        uiDataConsumer = new UIDataPublisher(index);
@@ -586,4 +586,15 @@ public class VisionModule {
            logger.error("Cannot set target model of non-reflective pipe! Ignoring...");
        }
    }
+
+    public void addCalibrationToConfig(CameraCalibrationCoefficients newCalibration) {
+        if (newCalibration != null) {
+            logger.info("Got new calibration for " + newCalibration.resolution);
+            visionSource.getSettables().getConfiguration().addCalibration(newCalibration);
+        } else {
+            logger.error("Got null calibration?");
+        }
+
+        saveAndBroadcastAll();
+    }
 }
--- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java
+++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java
@@ -28,6 +28,7 @@ import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.numbers.DoubleCouple;
 import org.photonvision.common.util.numbers.IntegerCouple;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
 import org.photonvision.vision.pipeline.PipelineType;
 import org.photonvision.vision.pipeline.UICalibrationData;
@@ -114,6 +115,10 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
                        parentModule.setPipeline(idx);
                        parentModule.saveAndBroadcastAll();
                        return;
+                    case "calibrationUploaded":
+                        if (newPropValue instanceof CameraCalibrationCoefficients)
+                            parentModule.addCalibrationToConfig((CameraCalibrationCoefficients) newPropValue);
+                        return;
                    case "robotOffsetPoint":
                        if (currentSettings instanceof AdvancedPipelineSettings) {
                            var curAdvSettings = (AdvancedPipelineSettings) currentSettings;
--- a/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStream.java
+++ b/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStream.java
@@ -17,6 +17,7 @@

 package org.photonvision.vision.videoStream;

+import edu.wpi.first.cscore.CameraServerJNI;
 import java.nio.ByteBuffer;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
@@ -51,7 +52,9 @@ public class SocketVideoStream implements Consumer<CVMat> {
    public SocketVideoStream(int portID) {
        this.portID = portID;
        oldSchoolServer =
-                new MJPGFrameConsumer("Port_" + Integer.toString(portID) + "_MJPEG_Server", portID);
+                new MJPGFrameConsumer(
+                        CameraServerJNI.getHostname() + "_Port_" + Integer.toString(portID) + "_MJPEG_Server",
+                        portID);
    }

    @Override
--- a/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
+++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
@@ -130,7 +130,7 @@ public class AprilTagTest {

        // these numbers are not *accurate*, but they are known and expected
        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
-        Assertions.assertEquals(4, pose.getTranslation().getX(), 0.2);
+        Assertions.assertEquals(4.14, pose.getTranslation().getX(), 0.2);
        Assertions.assertEquals(2, pose.getTranslation().getY(), 0.2);
        Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2);
    }
--- a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
+++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
@@ -37,6 +37,7 @@ import org.photonvision.common.util.TestUtils;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.frame.FrameDivisor;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.opencv.CVMat;
@@ -62,7 +63,7 @@ public class Calibrate3dPipeTest {
        FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe();
        findBoardCornersPipe.setParams(
                new FindBoardCornersPipe.FindCornersPipeParams(
-                        11, 4, UICalibrationData.BoardType.DOTBOARD, 15));
+                        11, 4, UICalibrationData.BoardType.DOTBOARD, 15, FrameDivisor.NONE));

        List<Triple<Size, Mat, Mat>> foundCornersList = new ArrayList<>();

@@ -264,6 +265,7 @@ public class Calibrate3dPipeTest {
        calibration3dPipeline.getSettings().boardHeight = (int) Math.round(boardDim.height);
        calibration3dPipeline.getSettings().boardWidth = (int) Math.round(boardDim.width);
        calibration3dPipeline.getSettings().gridSize = boardGridSize_m;
+        calibration3dPipeline.getSettings().streamingFrameDivisor = FrameDivisor.NONE;

        for (var file : directoryListing) {
            if (file.isFile()) {
--- a/photon-lib/build.gradle
+++ b/photon-lib/build.gradle
@@ -65,7 +65,7 @@ model {
                }
            }
            nativeUtils.useRequiredLibrary(it, "wpilib_shared")
-            nativeUtils.useRequiredLibrary(it, "vision_shared")
+            nativeUtils.useRequiredLibrary(it, "apriltag_shared")
        }
    }
    testSuites {
@@ -80,7 +80,7 @@ model {
            }

            nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
-            nativeUtils.useRequiredLibrary(it, "vision_shared")
+            nativeUtils.useRequiredLibrary(it, "apriltag_shared")
            nativeUtils.useRequiredLibrary(it, "googletest_static")
        }
    }
@@ -123,6 +123,10 @@ task writeCurrentVersion {

 build.dependsOn writeCurrentVersion

+tasks.withType(Javadoc) {
+    options.encoding = 'UTF-8'
+}
+
 apply from: "publish.gradle"

 def testNativeConfigName = 'wpilibTestNatives'
--- a/photon-lib/publish.gradle
+++ b/photon-lib/publish.gradle
@@ -1,6 +1,7 @@
 apply plugin: 'maven-publish'

-ext.licenseFile = files("$rootDir/LICENSE.txt")
+ext.licenseFile = files("$rootDir/LICENSE")
+ext.photonVersionFile = files("$projectDir/src/generate/native/include")

 def outputsFolder = file("$buildDir/outputs")
 def allOutputsFolder = file("$buildDir/allOutputs")
@@ -54,15 +55,19 @@ task cppHeadersZip(type: Zip) {
        into '/'
    }

+    from(photonVersionFile) {
+        into '/'
+    }
+
    from('src/main/native/include/') {
        into '/'
    }
 }

-task cppSourceZip(type: Zip) {
+task cppSourcesZip(type: Zip) {
    destinationDirectory = outputsFolder
    archiveBaseName = zipBaseName
-    classifier = "source"
+    classifier = "sources"

    from(licenseFile) {
        into '/'
@@ -75,8 +80,8 @@ task cppSourceZip(type: Zip) {

 build.dependsOn cppHeadersZip
 addTaskToCopyAllOutputs(cppHeadersZip)
-build.dependsOn cppSourceZip
-addTaskToCopyAllOutputs(cppSourceZip)
+build.dependsOn cppSourcesZip
+addTaskToCopyAllOutputs(cppSourcesZip)

 task sourcesJar(type: Jar, dependsOn: classes) {
    classifier = 'sources'
@@ -162,7 +167,7 @@ model {
                    artifact it
                }
                artifact cppHeadersZip
-                artifact cppSourceZip
+                artifact cppSourcesZip

                artifactId = "${baseArtifactId}-cpp"
                groupId artifactGroupId
--- a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
+++ b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
@@ -10,8 +10,8 @@
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
@@ -22,22 +22,26 @@
 * SOFTWARE.
 */

-#pragma once
+package org.photonvision;

-#include <Eigen/Core>
+import edu.wpi.first.math.geometry.Pose3d;

-namespace frc {
+/** An estimated pose based on pipeline result */
+public class EstimatedRobotPose {
+    /** The estimated pose */
+    public final Pose3d estimatedPose;

-template <int Size>
-using Vectord = Eigen::Vector<double, Size>;
+    /** The estimated time the frame used to derive the robot pose was taken */
+    public final double timestampSeconds;

-template <int Rows, int Cols,
-          int Options = Eigen::AutoAlign |
-                        ((Rows == 1 && Cols != 1) ? Eigen::RowMajor
-                         : (Cols == 1 && Rows != 1)
-                             ? Eigen::ColMajor
-                             : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION),
-          int MaxRows = Rows, int MaxCols = Cols>
-using Matrixd = Eigen::Matrix<double, Rows, Cols, Options, MaxRows, MaxCols>;
-
-}  // namespace frc
+    /**
+     * Constructs an EstimatedRobotPose
+     *
+     * @param estimatedPose estimated pose
+     * @param timestampSeconds timestamp of the estimate
+     */
+    public EstimatedRobotPose(Pose3d estimatedPose, double timestampSeconds) {
+        this.estimatedPose = estimatedPose;
+        this.timestampSeconds = timestampSeconds;
+    }
+}
--- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
@@ -24,7 +24,6 @@

 package org.photonvision;

-import edu.wpi.first.networktables.BooleanEntry;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleArrayPublisher;
@@ -50,7 +49,6 @@ public class PhotonCamera {

    protected final NetworkTable rootTable;
    RawSubscriber rawBytesEntry;
-    BooleanEntry driverModeEntry;
    BooleanPublisher driverModePublisher;
    BooleanSubscriber driverModeSubscriber;
    DoublePublisher latencyMillisEntry;
@@ -67,7 +65,6 @@ public class PhotonCamera {

    public void close() {
        rawBytesEntry.close();
-        driverModeEntry.close();
        driverModePublisher.close();
        driverModeSubscriber.close();
        latencyMillisEntry.close();
@@ -121,7 +118,8 @@ public class PhotonCamera {
                        .getRawTopic("rawBytes")
                        .subscribe(
                                "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
-        driverModeEntry = rootTable.getBooleanTopic("driverMode").getEntry(false);
+        driverModePublisher = rootTable.getBooleanTopic("driverMode").publish();
+        driverModeSubscriber = rootTable.getBooleanTopic("driverModeRequest").subscribe(false);
        inputSaveImgEntry = rootTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
        outputSaveImgEntry = rootTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
        pipelineIndexEntry = rootTable.getIntegerTopic("pipelineIndex").getEntry(0);
@@ -179,7 +177,7 @@ public class PhotonCamera {
     * @return Whether the camera is in driver mode.
     */
    public boolean getDriverMode() {
-        return driverModeEntry.get(false);
+        return driverModeSubscriber.get();
    }

    /**
@@ -188,7 +186,7 @@ public class PhotonCamera {
     * @param driverMode Whether to set driver mode.
     */
    public void setDriverMode(boolean driverMode) {
-        driverModeEntry.set(driverMode);
+        driverModePublisher.set(driverMode);
    }

    /**
--- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -0,0 +1,496 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.List;
+import java.util.Optional;
+import java.util.Set;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+/**
+ * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
+ * given timestamp on the field to produce a single robot in field pose, using the strategy set
+ * below. Example usage can be found in our apriltagExample example project.
+ */
+public class PhotonPoseEstimator {
+    /** Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. */
+    public enum PoseStrategy {
+        /** Choose the Pose with the lowest ambiguity. */
+        LOWEST_AMBIGUITY,
+
+        /** Choose the Pose which is closest to the camera height. */
+        CLOSEST_TO_CAMERA_HEIGHT,
+
+        /** Choose the Pose which is closest to a set Reference position. */
+        CLOSEST_TO_REFERENCE_POSE,
+
+        /** Choose the Pose which is closest to the last pose calculated */
+        CLOSEST_TO_LAST_POSE,
+
+        /** Choose the Pose with the lowest ambiguity. */
+        AVERAGE_BEST_TARGETS
+    }
+
+    private AprilTagFieldLayout fieldTags;
+    private PoseStrategy strategy;
+    private final PhotonCamera camera;
+    private final Transform3d robotToCamera;
+
+    private Pose3d lastPose;
+    private Pose3d referencePose;
+    private final Set<Integer> reportedErrors = new HashSet<>();
+
+    /**
+     * Create a new PhotonPoseEstimator.
+     *
+     * @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>.
+     * @param strategy The strategy it should use to determine the best pose.
+     * @param camera PhotonCameras and
+     * @param robotToCamera Transform3d from the center of the robot to the camera mount positions
+     *     (ie, robot ➔ camera) in the <a
+     *     href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
+     *     Coordinate System</a>.
+     */
+    public PhotonPoseEstimator(
+            AprilTagFieldLayout fieldTags,
+            PoseStrategy strategy,
+            PhotonCamera camera,
+            Transform3d robotToCamera) {
+        this.fieldTags = fieldTags;
+        this.strategy = strategy;
+        this.camera = camera;
+        this.robotToCamera = robotToCamera;
+    }
+
+    /**
+     * Get the AprilTagFieldLayout being used by the PositionEstimator.
+     *
+     * @return the AprilTagFieldLayout
+     */
+    public AprilTagFieldLayout getFieldTags() {
+        return fieldTags;
+    }
+
+    /**
+     * Set the AprilTagFieldLayout being used by the PositionEstimator.
+     *
+     * @param fieldTags the AprilTagFieldLayout
+     */
+    public void setFieldTags(AprilTagFieldLayout fieldTags) {
+        this.fieldTags = fieldTags;
+    }
+
+    /**
+     * Get the Position Estimation Strategy being used by the Position Estimator.
+     *
+     * @return the strategy
+     */
+    public PoseStrategy getStrategy() {
+        return strategy;
+    }
+
+    /**
+     * Set the Position Estimation Strategy used by the Position Estimator.
+     *
+     * @param strategy the strategy to set
+     */
+    public void setStrategy(PoseStrategy strategy) {
+        this.strategy = strategy;
+    }
+
+    /**
+     * Return the reference position that is being used by the estimator.
+     *
+     * @return the referencePose
+     */
+    public Pose3d getReferencePose() {
+        return referencePose;
+    }
+
+    /**
+     * Update the stored reference pose for use when using the <b>CLOSEST_TO_REFERENCE_POSE</b>
+     * strategy.
+     *
+     * @param referencePose the referencePose to set
+     */
+    public void setReferencePose(Pose3d referencePose) {
+        this.referencePose = referencePose;
+    }
+
+    /**
+     * Update the stored reference pose for use when using the <b>CLOSEST_TO_REFERENCE_POSE</b>
+     * strategy.
+     *
+     * @param referencePose the referencePose to set
+     */
+    public void setReferencePose(Pose2d referencePose) {
+        this.referencePose = new Pose3d(referencePose);
+    }
+
+    /**
+     * Update the stored last pose. Useful for setting the initial estimate when using the
+     * <b>CLOSEST_TO_LAST_POSE</b> strategy.
+     *
+     * @param lastPose the lastPose to set
+     */
+    public void setLastPose(Pose3d lastPose) {
+        this.lastPose = lastPose;
+    }
+
+    /**
+     * Update the stored last pose. Useful for setting the initial estimate when using the
+     * <b>CLOSEST_TO_LAST_POSE</b> strategy.
+     *
+     * @param lastPose the lastPose to set
+     */
+    public void setLastPose(Pose2d lastPose) {
+        setLastPose(new Pose3d(lastPose));
+    }
+
+    /**
+     * Poll data from the configured cameras and update the estimated position of the robot. Returns
+     * empty if there are no cameras set or no targets were found from the cameras.
+     *
+     * @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and
+     *     pipeline results used to create the estimate
+     */
+    public Optional<EstimatedRobotPose> update() {
+        if (camera == null) {
+            DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false);
+            return Optional.empty();
+        }
+
+        PhotonPipelineResult cameraResult = camera.getLatestResult();
+        if (!cameraResult.hasTargets()) {
+            return Optional.empty();
+        }
+
+        Optional<EstimatedRobotPose> estimatedPose;
+        switch (strategy) {
+            case LOWEST_AMBIGUITY:
+                estimatedPose = lowestAmbiguityStrategy(cameraResult);
+                break;
+            case CLOSEST_TO_CAMERA_HEIGHT:
+                estimatedPose = closestToCameraHeightStrategy(cameraResult);
+                break;
+            case CLOSEST_TO_LAST_POSE:
+                setReferencePose(lastPose);
+            case CLOSEST_TO_REFERENCE_POSE:
+                estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose);
+                break;
+            case AVERAGE_BEST_TARGETS:
+                estimatedPose = averageBestTargetsStrategy(cameraResult);
+                break;
+            default:
+                DriverStation.reportError(
+                        "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false);
+                return Optional.empty();
+        }
+
+        if (estimatedPose.isEmpty()) {
+            lastPose = null;
+        }
+
+        return estimatedPose;
+    }
+
+    /**
+     * Return the estimated position of the robot with the lowest position ambiguity from a List of
+     * pipeline results.
+     *
+     * @param result pipeline result
+     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
+     *     estimation.
+     */
+    private Optional<EstimatedRobotPose> lowestAmbiguityStrategy(PhotonPipelineResult result) {
+        PhotonTrackedTarget lowestAmbiguityTarget = null;
+
+        double lowestAmbiguityScore = 10;
+
+        for (PhotonTrackedTarget target : result.targets) {
+            double targetPoseAmbiguity = target.getPoseAmbiguity();
+            // Make sure the target is a Fiducial target.
+            if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) {
+                lowestAmbiguityScore = targetPoseAmbiguity;
+                lowestAmbiguityTarget = target;
+            }
+        }
+
+        // Although there are confirmed to be targets, none of them may be fiducial
+        // targets.
+        if (lowestAmbiguityTarget == null) return Optional.empty();
+
+        int targetFiducialId = lowestAmbiguityTarget.getFiducialId();
+
+        Optional<Pose3d> targetPosition = fieldTags.getTagPose(targetFiducialId);
+
+        if (targetPosition.isEmpty()) {
+            reportFiducialPoseError(targetFiducialId);
+            return Optional.empty();
+        }
+
+        return Optional.of(
+                new EstimatedRobotPose(
+                        targetPosition
+                                .get()
+                                .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
+                                .transformBy(robotToCamera.inverse()),
+                        result.getTimestampSeconds()));
+    }
+
+    /**
+     * Return the estimated position of the robot using the target with the lowest delta height
+     * difference between the estimated and actual height of the camera.
+     *
+     * @param result pipeline result
+     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
+     *     estimation.
+     */
+    private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelineResult result) {
+        double smallestHeightDifference = 10e9;
+        EstimatedRobotPose closestHeightTarget = null;
+
+        for (PhotonTrackedTarget target : result.targets) {
+            int targetFiducialId = target.getFiducialId();
+
+            // Don't report errors for non-fiducial targets. This could also be resolved by
+            // adding -1 to
+            // the initial HashSet.
+            if (targetFiducialId == -1) continue;
+
+            Optional<Pose3d> targetPosition = fieldTags.getTagPose(target.getFiducialId());
+
+            if (targetPosition.isEmpty()) {
+                reportFiducialPoseError(target.getFiducialId());
+                continue;
+            }
+
+            double alternateTransformDelta =
+                    Math.abs(
+                            robotToCamera.getZ()
+                                    - targetPosition
+                                            .get()
+                                            .transformBy(target.getAlternateCameraToTarget().inverse())
+                                            .getZ());
+            double bestTransformDelta =
+                    Math.abs(
+                            robotToCamera.getZ()
+                                    - targetPosition
+                                            .get()
+                                            .transformBy(target.getBestCameraToTarget().inverse())
+                                            .getZ());
+
+            if (alternateTransformDelta < smallestHeightDifference) {
+                smallestHeightDifference = alternateTransformDelta;
+                closestHeightTarget =
+                        new EstimatedRobotPose(
+                                targetPosition
+                                        .get()
+                                        .transformBy(target.getAlternateCameraToTarget().inverse())
+                                        .transformBy(robotToCamera.inverse()),
+                                result.getTimestampSeconds());
+            }
+
+            if (bestTransformDelta < smallestHeightDifference) {
+                smallestHeightDifference = bestTransformDelta;
+                closestHeightTarget =
+                        new EstimatedRobotPose(
+                                targetPosition
+                                        .get()
+                                        .transformBy(target.getBestCameraToTarget().inverse())
+                                        .transformBy(robotToCamera.inverse()),
+                                result.getTimestampSeconds());
+            }
+        }
+
+        // Need to null check here in case none of the provided targets are fiducial.
+        return Optional.ofNullable(closestHeightTarget);
+    }
+
+    /**
+     * Return the estimated position of the robot using the target with the lowest delta in the vector
+     * magnitude between it and the reference pose.
+     *
+     * @param result pipeline result
+     * @param referencePose reference pose to check vector magnitude difference against.
+     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
+     *     estimation.
+     */
+    private Optional<EstimatedRobotPose> closestToReferencePoseStrategy(
+            PhotonPipelineResult result, Pose3d referencePose) {
+        if (referencePose == null) {
+            DriverStation.reportError(
+                    "[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!",
+                    false);
+            return Optional.empty();
+        }
+
+        double smallestPoseDelta = 10e9;
+        EstimatedRobotPose lowestDeltaPose = null;
+
+        for (PhotonTrackedTarget target : result.targets) {
+            int targetFiducialId = target.getFiducialId();
+
+            // Don't report errors for non-fiducial targets. This could also be resolved by
+            // adding -1 to
+            // the initial HashSet.
+            if (targetFiducialId == -1) continue;
+
+            Optional<Pose3d> targetPosition = fieldTags.getTagPose(target.getFiducialId());
+
+            if (targetPosition.isEmpty()) {
+                reportFiducialPoseError(targetFiducialId);
+                continue;
+            }
+
+            Pose3d altTransformPosition =
+                    targetPosition
+                            .get()
+                            .transformBy(target.getAlternateCameraToTarget().inverse())
+                            .transformBy(robotToCamera.inverse());
+            Pose3d bestTransformPosition =
+                    targetPosition
+                            .get()
+                            .transformBy(target.getBestCameraToTarget().inverse())
+                            .transformBy(robotToCamera.inverse());
+
+            double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition));
+            double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition));
+
+            if (altDifference < smallestPoseDelta) {
+                smallestPoseDelta = altDifference;
+                lowestDeltaPose =
+                        new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds());
+            }
+            if (bestDifference < smallestPoseDelta) {
+                smallestPoseDelta = bestDifference;
+                lowestDeltaPose =
+                        new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds());
+            }
+        }
+        return Optional.ofNullable(lowestDeltaPose);
+    }
+
+    /**
+     * Return the average of the best target poses using ambiguity as weight.
+     *
+     * @param result pipeline result
+     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
+     *     estimation.
+     */
+    private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineResult result) {
+        List<Pair<PhotonTrackedTarget, Pose3d>> estimatedRobotPoses = new ArrayList<>();
+        double totalAmbiguity = 0;
+
+        for (PhotonTrackedTarget target : result.targets) {
+            int targetFiducialId = target.getFiducialId();
+
+            // Don't report errors for non-fiducial targets. This could also be resolved by
+            // adding -1 to
+            // the initial HashSet.
+            if (targetFiducialId == -1) continue;
+
+            Optional<Pose3d> targetPosition = fieldTags.getTagPose(target.getFiducialId());
+
+            if (targetPosition.isEmpty()) {
+                reportFiducialPoseError(targetFiducialId);
+                continue;
+            }
+
+            double targetPoseAmbiguity = target.getPoseAmbiguity();
+
+            // Pose ambiguity is 0, use that pose
+            if (targetPoseAmbiguity == 0) {
+                return Optional.of(
+                        new EstimatedRobotPose(
+                                targetPosition
+                                        .get()
+                                        .transformBy(target.getBestCameraToTarget().inverse())
+                                        .transformBy(robotToCamera.inverse()),
+                                result.getTimestampSeconds()));
+            }
+
+            totalAmbiguity += 1.0 / target.getPoseAmbiguity();
+
+            estimatedRobotPoses.add(
+                    new Pair<>(
+                            target,
+                            targetPosition
+                                    .get()
+                                    .transformBy(target.getBestCameraToTarget().inverse())
+                                    .transformBy(robotToCamera.inverse())));
+        }
+
+        // Take the average
+
+        Translation3d transform = new Translation3d();
+        Rotation3d rotation = new Rotation3d();
+
+        if (estimatedRobotPoses.isEmpty()) return Optional.empty();
+
+        for (Pair<PhotonTrackedTarget, Pose3d> pair : estimatedRobotPoses) {
+            // Total ambiguity is non-zero confirmed because if it was zero, that pose was
+            // returned.
+            double weight = (1.0 / pair.getFirst().getPoseAmbiguity()) / totalAmbiguity;
+            Pose3d estimatedPose = pair.getSecond();
+            transform = transform.plus(estimatedPose.getTranslation().times(weight));
+            rotation = rotation.plus(estimatedPose.getRotation().times(weight));
+        }
+
+        return Optional.of(
+                new EstimatedRobotPose(new Pose3d(transform, rotation), result.getTimestampSeconds()));
+    }
+
+    /**
+     * Difference is defined as the vector magnitude between the two poses
+     *
+     * @return The absolute "difference" (>=0) between two Pose3ds.
+     */
+    private double calculateDifference(Pose3d x, Pose3d y) {
+        return x.getTranslation().getDistance(y.getTranslation());
+    }
+
+    private void reportFiducialPoseError(int fiducialId) {
+        if (!reportedErrors.contains(fiducialId)) {
+            DriverStation.reportError(
+                    "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false);
+            reportedErrors.add(fiducialId);
+        }
+    }
+}
--- a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java
+++ b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java
@@ -40,6 +40,8 @@ import java.util.Set;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;

+/** @deprecated Use {@link PhotonPoseEstimator} */
+@Deprecated
 public class RobotPoseEstimator {
    /**
     *
@@ -77,7 +79,7 @@ public class RobotPoseEstimator {
     *     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).
+     *     the center of the robot to the camera mount positions (ie, robot ➔ camera).
     */
    public RobotPoseEstimator(
            AprilTagFieldLayout aprilTags,
@@ -387,7 +389,7 @@ public class RobotPoseEstimator {
    }

    /**
-     * UPdate the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE
+     * Update the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE
     *
     * @param lastPose the lastPose to set
     */
@@ -398,7 +400,7 @@ public class RobotPoseEstimator {
    private void reportFiducialPoseError(int fiducialId) {
        if (!reportedErrors.contains(fiducialId)) {
            DriverStation.reportError(
-                    "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " + fiducialId, false);
+                    "[RobotPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false);
            reportedErrors.add(fiducialId);
        }
    }
--- a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
+++ b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
@@ -119,7 +119,6 @@ public class SimVisionSystem {
        dbgField
                .getObject("Target " + Integer.toString(target.targetID))
                .setPose(target.targetPose.toPose2d());
-        ;
    }

    /**
@@ -139,6 +138,15 @@ public class SimVisionSystem {
        }
    }

+    /**
+     * Clears all sim vision targets. This is useful for switching alliances and needing to repopulate
+     * the sim targets. NOTE: Old targets will still show on the Field2d unless overwritten by new
+     * targets with the same ID
+     */
+    public void clearVisionTargets() {
+        tgtList.clear();
+    }
+
    /**
     * Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or
     * turret or some other mobile platform.
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
@@ -41,8 +41,7 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
      rootTable(mainTable->GetSubTable(cameraName)),
      rawBytesEntry(
          rootTable->GetRawTopic("rawBytes")
-              .Subscribe("raw", {}, {.periodic = 0.01, .sendAll = true})),
-      driverModeEntry(rootTable->GetBooleanTopic("driverMode").Publish()),
+              .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})),
      inputSaveImgEntry(
          rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()),
      inputSaveImgSubscriber(
@@ -56,6 +55,8 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
      versionEntry(mainTable->GetStringTopic("version").Subscribe("")),
      driverModeSubscriber(
          rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
+      driverModePublisher(
+          rootTable->GetBooleanTopic("driverModeRequest").Publish()),
      pipelineIndexSubscriber(
          rootTable->GetIntegerTopic("pipelineIndex").Subscribe(-1)),
      ledModeSubscriber(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
@@ -92,7 +93,7 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
 }

 void PhotonCamera::SetDriverMode(bool driverMode) {
-  driverModeEntry.Set(driverMode);
+  driverModePublisher.Set(driverMode);
 }

 void PhotonCamera::TakeInputSnapshot() {
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
@@ -0,0 +1,270 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "photonlib/PhotonPoseEstimator.h"
+
+#include <iostream>
+#include <limits>
+#include <map>
+#include <span>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <frc/Errors.h>
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Rotation3d.h>
+#include <frc/geometry/Transform3d.h>
+#include <units/time.h>
+
+#include "photonlib/PhotonCamera.h"
+#include "photonlib/PhotonPipelineResult.h"
+#include "photonlib/PhotonTrackedTarget.h"
+
+namespace photonlib {
+PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
+                                         PoseStrategy strat, PhotonCamera&& cam,
+                                         frc::Transform3d robotToCamera)
+    : aprilTags(tags),
+      strategy(strat),
+      camera(std::move(cam)),
+      m_robotToCamera(robotToCamera),
+      lastPose(frc::Pose3d()),
+      referencePose(frc::Pose3d()) {}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
+  auto result = camera.GetLatestResult();
+
+  if (!result.HasTargets()) {
+    return std::nullopt;
+  }
+
+  std::optional<EstimatedRobotPose> ret = std::nullopt;
+
+  switch (strategy) {
+    case LOWEST_AMBIGUITY:
+      ret = LowestAmbiguityStrategy(result);
+      break;
+    case CLOSEST_TO_CAMERA_HEIGHT:
+      ret = ClosestToCameraHeightStrategy(result);
+      break;
+    case CLOSEST_TO_REFERENCE_POSE:
+      ret = ClosestToReferencePoseStrategy(result);
+      break;
+    case CLOSEST_TO_LAST_POSE:
+      SetReferencePose(lastPose);
+      ret = ClosestToReferencePoseStrategy(result);
+      break;
+    case AVERAGE_BEST_TARGETS:
+      ret = AverageBestTargetsStrategy(result);
+      break;
+    default:
+      FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
+                      "");
+      return std::nullopt;
+  }
+
+  if (!ret) {
+    // TODO
+  }
+
+  return ret;
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
+    PhotonPipelineResult result) {
+  int lowestAJ = -1;
+  double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
+  auto targets = result.GetTargets();
+  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
+    if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) {
+      lowestAJ = j;
+      lowestAmbiguityScore = targets[j].GetPoseAmbiguity();
+    }
+  }
+
+  if (lowestAJ == -1) {
+    return std::nullopt;
+  }
+
+  PhotonTrackedTarget bestTarget = targets[lowestAJ];
+
+  std::optional<frc::Pose3d> fiducialPose =
+      aprilTags.GetTagPose(bestTarget.GetFiducialId());
+  if (!fiducialPose) {
+    FRC_ReportError(frc::warn::Warning,
+                    "Tried to get pose of unknown April Tag: {}",
+                    bestTarget.GetFiducialId());
+    return std::nullopt;
+  }
+
+  return EstimatedRobotPose{
+      fiducialPose.value()
+          .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
+          .TransformBy(m_robotToCamera.Inverse()),
+      result.GetTimestamp()};
+}
+
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::ClosestToCameraHeightStrategy(
+    PhotonPipelineResult result) {
+  units::meter_t smallestHeightDifference =
+      units::meter_t(std::numeric_limits<double>::infinity());
+
+  std::optional<EstimatedRobotPose> pose = std::nullopt;
+
+  for (auto& target : result.GetTargets()) {
+    std::optional<frc::Pose3d> fiducialPose =
+        aprilTags.GetTagPose(target.GetFiducialId());
+    if (!fiducialPose) {
+      FRC_ReportError(frc::warn::Warning,
+                      "Tried to get pose of unknown April Tag: {}",
+                      target.GetFiducialId());
+      continue;
+    }
+    frc::Pose3d targetPose = fiducialPose.value();
+
+    units::meter_t alternativeDifference = units::math::abs(
+        m_robotToCamera.Z() -
+        targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
+            .Z());
+
+    units::meter_t bestDifference = units::math::abs(
+        m_robotToCamera.Z() -
+        targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
+
+    if (alternativeDifference < smallestHeightDifference) {
+      smallestHeightDifference = alternativeDifference;
+      pose = EstimatedRobotPose{
+          targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
+              .TransformBy(m_robotToCamera.Inverse()),
+          result.GetTimestamp()};
+    }
+    if (bestDifference < smallestHeightDifference) {
+      smallestHeightDifference = bestDifference;
+      pose = EstimatedRobotPose{
+          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
+              .TransformBy(m_robotToCamera.Inverse()),
+          result.GetTimestamp()};
+    }
+  }
+
+  return pose;
+}
+
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::ClosestToReferencePoseStrategy(
+    PhotonPipelineResult result) {
+  units::meter_t smallestDifference =
+      units::meter_t(std::numeric_limits<double>::infinity());
+  units::second_t stateTimestamp = units::second_t(0);
+  frc::Pose3d pose = lastPose;
+
+  auto targets = result.GetTargets();
+  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
+    PhotonTrackedTarget target = targets[j];
+    std::optional<frc::Pose3d> fiducialPose =
+        aprilTags.GetTagPose(target.GetFiducialId());
+    if (!fiducialPose) {
+      FRC_ReportError(frc::warn::Warning,
+                      "Tried to get pose of unknown April Tag: {}",
+                      target.GetFiducialId());
+      continue;
+    }
+    frc::Pose3d targetPose = fiducialPose.value();
+
+    const auto altPose =
+        targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
+            .TransformBy(m_robotToCamera.Inverse());
+    const auto bestPose =
+        targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
+            .TransformBy(m_robotToCamera.Inverse());
+
+    units::meter_t alternativeDifference = units::math::abs(
+        referencePose.Translation().Distance(altPose.Translation()));
+    units::meter_t bestDifference = units::math::abs(
+        referencePose.Translation().Distance(bestPose.Translation()));
+    if (alternativeDifference < smallestDifference) {
+      smallestDifference = alternativeDifference;
+      pose = altPose;
+      stateTimestamp = result.GetTimestamp();
+    }
+
+    if (bestDifference < smallestDifference) {
+      smallestDifference = bestDifference;
+      pose = bestPose;
+      stateTimestamp = result.GetTimestamp();
+    }
+  }
+
+  return EstimatedRobotPose{pose, stateTimestamp};
+}
+
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
+  std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
+      tempPoses;
+  double totalAmbiguity = 0;
+
+  auto targets = result.GetTargets();
+  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
+    PhotonTrackedTarget target = targets[j];
+    std::optional<frc::Pose3d> fiducialPose =
+        aprilTags.GetTagPose(target.GetFiducialId());
+    if (!fiducialPose) {
+      FRC_ReportError(frc::warn::Warning,
+                      "Tried to get pose of unknown April Tag: {}",
+                      target.GetFiducialId());
+      continue;
+    }
+
+    frc::Pose3d targetPose = fiducialPose.value();
+    // Ambiguity = 0, use that pose
+    if (target.GetPoseAmbiguity() == 0) {
+      return EstimatedRobotPose{
+          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
+              .TransformBy(m_robotToCamera.Inverse()),
+          result.GetLatency()};
+    }
+    totalAmbiguity += 1. / target.GetPoseAmbiguity();
+
+    tempPoses.push_back(std::make_pair(
+        targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
+        std::make_pair(target.GetPoseAmbiguity(), result.GetTimestamp())));
+  }
+
+  frc::Translation3d transform = frc::Translation3d();
+  frc::Rotation3d rotation = frc::Rotation3d();
+
+  for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
+       tempPoses) {
+    double weight = (1. / pair.second.first) / totalAmbiguity;
+    transform = transform + pair.first.Translation() * weight;
+    rotation = rotation + pair.first.Rotation() * weight;
+  }
+
+  return EstimatedRobotPose{frc::Pose3d(transform, rotation),
+                            result.GetTimestamp()};
+}
+}  // namespace photonlib
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
@@ -38,7 +38,8 @@ PhotonTrackedTarget::PhotonTrackedTarget(
    double yaw, double pitch, double area, double skew, int id,
    const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
    double ambiguity,
-    const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners)
+    const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners,
+    const std::vector<std::pair<double, double>> detectedCorners)
    : yaw(yaw),
      pitch(pitch),
      area(area),
@@ -47,7 +48,8 @@ PhotonTrackedTarget::PhotonTrackedTarget(
      bestCameraToTarget(pose),
      altCameraToTarget(alternatePose),
      poseAmbiguity(ambiguity),
-      minAreaRectCorners(minAreaRectCorners) {}
+      minAreaRectCorners(minAreaRectCorners),
+      detectedCorners(detectedCorners) {}

 bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
  return other.yaw == yaw && other.pitch == pitch && other.area == area &&
--- a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
@@ -54,11 +54,12 @@ RobotPoseEstimator::RobotPoseEstimator(
      lastPose(frc::Pose3d()),
      referencePose(frc::Pose3d()) {}

-std::pair<frc::Pose3d, units::millisecond_t> RobotPoseEstimator::Update() {
+std::pair<frc::Pose3d, units::second_t> RobotPoseEstimator::Update() {
  if (cameras.empty()) {
-    return std::make_pair(lastPose, units::millisecond_t(0));
+    return std::make_pair(lastPose, units::second_t(0));
  }
-  std::pair<frc::Pose3d, units::millisecond_t> pair;
+
+  std::pair<frc::Pose3d, units::second_t> pair;
  switch (strategy) {
    case LOWEST_AMBIGUITY:
      pair = LowestAmbiguityStrategy();
@@ -73,7 +74,7 @@ std::pair<frc::Pose3d, units::millisecond_t> RobotPoseEstimator::Update() {
      lastPose = pair.first;
      return pair;
    case CLOSEST_TO_LAST_POSE:
-      referencePose = lastPose;
+      SetReferencePose(lastPose);
      pair = ClosestToReferencePoseStrategy();
      lastPose = pair.first;
      return pair;
@@ -85,10 +86,11 @@ std::pair<frc::Pose3d, units::millisecond_t> RobotPoseEstimator::Update() {
      FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
                      "");
  }
-  return std::make_pair(lastPose, units::millisecond_t(0));
+
+  return std::make_pair(lastPose, units::second_t(0));
 }

-std::pair<frc::Pose3d, units::millisecond_t>
+std::pair<frc::Pose3d, units::second_t>
 RobotPoseEstimator::LowestAmbiguityStrategy() {
  int lowestAI = -1;
  int lowestAJ = -1;
@@ -107,7 +109,7 @@ RobotPoseEstimator::LowestAmbiguityStrategy() {
  }

  if (lowestAI == -1 || lowestAJ == -1) {
-    return std::make_pair(lastPose, units::millisecond_t(0));
+    return std::make_pair(lastPose, units::second_t(0));
  }

  PhotonTrackedTarget bestTarget =
@@ -119,20 +121,21 @@ RobotPoseEstimator::LowestAmbiguityStrategy() {
    FRC_ReportError(frc::warn::Warning,
                    "Tried to get pose of unknown April Tag: {}",
                    bestTarget.GetFiducialId());
-    return std::make_pair(lastPose, units::millisecond_t(0));
+    return std::make_pair(lastPose, units::second_t(0));
  }

  return std::make_pair(
      fiducialPose.value()
          .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
          .TransformBy(cameras[lowestAI].second.Inverse()),
-      cameras[lowestAI].first->GetLatestResult().GetLatency() / 1000.);
+      cameras[lowestAI].first->GetLatestResult().GetTimestamp());
 }
-std::pair<frc::Pose3d, units::millisecond_t>
+
+std::pair<frc::Pose3d, units::second_t>
 RobotPoseEstimator::ClosestToCameraHeightStrategy() {
  units::meter_t smallestHeightDifference =
      units::meter_t(std::numeric_limits<double>::infinity());
-  units::millisecond_t milli = units::millisecond_t(0);
+  units::second_t stateTimestamp = units::second_t(0);
  frc::Pose3d pose = lastPose;

  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
@@ -161,22 +164,23 @@ RobotPoseEstimator::ClosestToCameraHeightStrategy() {
        smallestHeightDifference = alternativeDifference;
        pose = targetPose.TransformBy(
            target.GetAlternateCameraToTarget().Inverse());
-        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
      }
      if (bestDifference < smallestHeightDifference) {
        smallestHeightDifference = bestDifference;
        pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
-        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
      }
    }
  }
-  return std::make_pair(pose, milli);
+  return std::make_pair(pose, stateTimestamp);
 }
-std::pair<frc::Pose3d, units::millisecond_t>
+
+std::pair<frc::Pose3d, units::second_t>
 RobotPoseEstimator::ClosestToReferencePoseStrategy() {
  units::meter_t smallestDifference =
      units::meter_t(std::numeric_limits<double>::infinity());
-  units::millisecond_t milli = units::millisecond_t(0);
+  units::second_t stateTimestamp = units::second_t(0);
  frc::Pose3d pose = lastPose;

  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
@@ -207,29 +211,32 @@ RobotPoseEstimator::ClosestToReferencePoseStrategy() {
        smallestDifference = alternativeDifference;
        pose = targetPose.TransformBy(
            target.GetAlternateCameraToTarget().Inverse());
-        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
      }

      if (bestDifference < smallestDifference) {
        smallestDifference = bestDifference;
        pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
-        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
      }
    }
  }
-  return std::make_pair(pose, milli);
+
+  return std::make_pair(pose, stateTimestamp);
 }

-std::pair<frc::Pose3d, units::millisecond_t>
+std::pair<frc::Pose3d, units::second_t>
 RobotPoseEstimator::AverageBestTargetsStrategy() {
-  std::vector<std::pair<frc::Pose3d, std::pair<double, units::millisecond_t>>>
+  std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
      tempPoses;
  double totalAmbiguity = 0;
+  units::second_t timstampSum = units::second_t(0);

  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
    std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
    std::span<const PhotonTrackedTarget> targets =
        p.first->GetLatestResult().GetTargets();
+    timstampSum += p.first->GetLatestResult().GetTimestamp();
    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
      PhotonTrackedTarget target = targets[j];
      std::optional<frc::Pose3d> fiducialPose =
@@ -255,20 +262,21 @@ RobotPoseEstimator::AverageBestTargetsStrategy() {
      tempPoses.push_back(std::make_pair(
          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
          std::make_pair(target.GetPoseAmbiguity(),
-                         p.first->GetLatestResult().GetLatency() / 1000.)));
+                         p.first->GetLatestResult().GetTimestamp())));
    }
  }
+
  frc::Translation3d transform = frc::Translation3d();
  frc::Rotation3d rotation = frc::Rotation3d();
-  units::millisecond_t latency = units::millisecond_t(0);

-  for (std::pair<frc::Pose3d, std::pair<double, units::millisecond_t>>& pair :
+  for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
       tempPoses) {
    double weight = (1. / pair.second.first) / totalAmbiguity;
    transform = transform + pair.first.Translation() * weight;
    rotation = rotation + pair.first.Rotation() * weight;
-    latency += pair.second.second * weight;
  }
-  return std::make_pair(frc::Pose3d(transform, rotation), latency);
+
+  return std::make_pair(frc::Pose3d(transform, rotation),
+                        timstampSum / cameras.size());
 }
 }  // namespace photonlib
--- a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h
+++ b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h
@@ -68,6 +68,8 @@ class PhotonCamera {
   */
  explicit PhotonCamera(const std::string_view cameraName);

+  PhotonCamera(PhotonCamera&&) = default;
+
  virtual ~PhotonCamera() = default;

  /**
@@ -166,7 +168,6 @@ class PhotonCamera {
  std::shared_ptr<nt::NetworkTable> mainTable;
  std::shared_ptr<nt::NetworkTable> rootTable;
  nt::RawSubscriber rawBytesEntry;
-  nt::BooleanPublisher driverModeEntry;
  nt::IntegerPublisher inputSaveImgEntry;
  nt::IntegerSubscriber inputSaveImgSubscriber;
  nt::IntegerPublisher outputSaveImgEntry;
@@ -176,6 +177,7 @@ class PhotonCamera {
  nt::StringSubscriber versionEntry;

  nt::BooleanSubscriber driverModeSubscriber;
+  nt::BooleanPublisher driverModePublisher;
  nt::IntegerSubscriber pipelineIndexSubscriber;
  nt::IntegerSubscriber ledModeSubscriber;

--- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
+++ b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
@@ -0,0 +1,196 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <map>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Transform3d.h>
+
+#include "photonlib/PhotonCamera.h"
+
+namespace photonlib {
+enum PoseStrategy : int {
+  LOWEST_AMBIGUITY,
+  CLOSEST_TO_CAMERA_HEIGHT,
+  CLOSEST_TO_REFERENCE_POSE,
+  CLOSEST_TO_LAST_POSE,
+  AVERAGE_BEST_TARGETS
+};
+
+struct EstimatedRobotPose {
+  /** The estimated pose */
+  frc::Pose3d estimatedPose;
+  /** The estimated time the frame used to derive the robot pose was taken, in
+   * the same timebase as the RoboRIO FPGA Timestamp */
+  units::second_t timestamp;
+
+  EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_)
+      : estimatedPose(pose_), timestamp(time_) {}
+};
+
+/**
+ * The PhotonPoseEstimator class filters or combines readings from all the
+ * fiducials visible at a given timestamp on the field to produce a single robot
+ * in field pose, using the strategy set below. Example usage can be found in
+ * our apriltagExample example project.
+ */
+class PhotonPoseEstimator {
+ public:
+  using map_value_type =
+      std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
+  using size_type = std::vector<map_value_type>::size_type;
+
+  /**
+   * Create a new PhotonPoseEstimator.
+   *
+   * <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 strategy The strategy it should use to determine the best pose.
+   * @param camera PhotonCameras and
+   * @param robotToCamera Transform3d from the center of the robot to the camera
+   * mount positions (ie, robot ➔ camera).
+   */
+  explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
+                               PoseStrategy strategy, PhotonCamera&& camera,
+                               frc::Transform3d robotToCamera);
+
+  /**
+   * Get the AprilTagFieldLayout being used by the PositionEstimator.
+   *
+   * @return the AprilTagFieldLayout
+   */
+  frc::AprilTagFieldLayout GetFieldLayout() const { return aprilTags; }
+
+  /**
+   * Get the Position Estimation Strategy being used by the Position Estimator.
+   *
+   * @return the strategy
+   */
+  PoseStrategy GetPoseStrategy() const { return strategy; }
+
+  /**
+   * Set the Position Estimation Strategy used by the Position Estimator.
+   *
+   * @param strategy the strategy to set
+   */
+  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
+
+  /**
+   * Return the reference position that is being used by the estimator.
+   *
+   * @return the referencePose
+   */
+  frc::Pose3d GetReferencePose() const { return referencePose; }
+
+  /**
+   * Update the stored reference pose for use when using the
+   * CLOSEST_TO_REFERENCE_POSE strategy.
+   *
+   * @param referencePose the referencePose to set
+   */
+  inline void SetReferencePose(frc::Pose3d referencePose) {
+    this->referencePose = referencePose;
+  }
+
+  /**
+   * Update the stored last pose. Useful for setting the initial estimate when
+   * using the CLOSEST_TO_LAST_POSE strategy.
+   *
+   * @param lastPose the lastPose to set
+   */
+  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
+
+  /**
+   * Update the pose estimator. Internally grabs a new PhotonPipelineResult from
+   * the camera and process it.
+   */
+  std::optional<EstimatedRobotPose> Update();
+
+  inline PhotonCamera& GetCamera() { return camera; }
+
+ private:
+  frc::AprilTagFieldLayout aprilTags;
+  PoseStrategy strategy;
+
+  PhotonCamera camera;
+  frc::Transform3d m_robotToCamera;
+
+  frc::Pose3d lastPose;
+  frc::Pose3d referencePose;
+
+  /**
+   * Return the estimated position of the robot with the lowest position
+   * ambiguity from a List of pipeline results.
+   *
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::optional<EstimatedRobotPose> LowestAmbiguityStrategy(
+      PhotonPipelineResult result);
+
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta height difference between the estimated and actual height of the
+   * camera.
+   *
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::optional<EstimatedRobotPose> ClosestToCameraHeightStrategy(
+      PhotonPipelineResult result);
+
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta in the vector magnitude between it and the reference pose.
+   *
+   * @param referencePose reference pose to check vector magnitude difference
+   * against.
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
+      PhotonPipelineResult result);
+
+  /**
+   * Return the average of the best target poses using ambiguity as weight.
+
+   * @return the estimated position of the robot in the FCS and the estimated
+   timestamp of this
+   *     estimation.
+   */
+  std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
+      PhotonPipelineResult result);
+};
+
+}  // namespace photonlib
--- a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h
+++ b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h
@@ -59,7 +59,8 @@ class PhotonTrackedTarget {
      double yaw, double pitch, double area, double skew, int fiducialID,
      const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
      double ambiguity,
-      const wpi::SmallVector<std::pair<double, double>, 4> corners);
+      const wpi::SmallVector<std::pair<double, double>, 4> corners,
+      const std::vector<std::pair<double, double>> detectedCorners);

  /**
   * Returns the target yaw (positive-left).
--- a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h
+++ b/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h
@@ -48,7 +48,10 @@ enum PoseStrategy : int {
 };

 /**
- * A managing class to determine how an estimated pose should be chosen.
+ * The RobotPoseEstimator class filters or combines readings from all the
+ * fiducials visible at a given timestamp on the field to produce a single robot
+ * in field pose, using the strategy set below. Example usage can be found in
+ * our apriltagExample example project.
 */
 class RobotPoseEstimator {
 public:
@@ -59,44 +62,81 @@ class RobotPoseEstimator {
  /**
   * Create a new RobotPoseEstimator.
   *
-   * @param aprilTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs
-   * to Pose3ds with respect to the FIRST field.
+   * <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 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).
+   * Transform3ds from the center of the robot to the cameras.
   */
  explicit RobotPoseEstimator(
      std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
      PoseStrategy strategy, std::vector<map_value_type> cameras);

  /**
-   * Update the estimated pose using the selected strategy.
+   * Get the AprilTagFieldLayout being used by the PositionEstimator.
   *
-   * @return The updated estimated pose and the latency in milliseconds.
+   * @return the AprilTagFieldLayout
   */
-  std::pair<frc::Pose3d, units::millisecond_t> Update();
-
-  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
-
-  inline void SetReferencePose(frc::Pose3d referencePose) {
-    this->referencePose = referencePose;
+  std::shared_ptr<frc::AprilTagFieldLayout> getFieldLayout() const {
+    return aprilTags;
  }

-  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
-
+  /**
+   * Set the cameras to be used by the PoseEstimator.
+   *
+   * @param cameras cameras to set.
+   */
  inline void SetCameras(
      const std::vector<std::pair<std::shared_ptr<PhotonCamera>,
                                  frc::Transform3d>>& cameras) {
    this->cameras = cameras;
  }

+  /**
+   * Get the Position Estimation Strategy being used by the Position Estimator.
+   *
+   * @return the strategy
+   */
  PoseStrategy GetPoseStrategy() const { return strategy; }

-  frc::Pose3d GetLastPose() const { return lastPose; }
+  /**
+   * Set the Position Estimation Strategy used by the Position Estimator.
+   *
+   * @param strategy the strategy to set
+   */
+  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }

+  /**
+   * Return the reference position that is being used by the estimator.
+   *
+   * @return the referencePose
+   */
  frc::Pose3d GetReferencePose() const { return referencePose; }

+  /**
+   * Update the stored reference pose for use when using the
+   * CLOSEST_TO_REFERENCE_POSE strategy.
+   *
+   * @param referencePose the referencePose to set
+   */
+  inline void SetReferencePose(frc::Pose3d referencePose) {
+    this->referencePose = referencePose;
+  }
+
+  /**
+   * Update the stored last pose. Useful for setting the initial estimate when
+   * using the CLOSEST_TO_LAST_POSE strategy.
+   *
+   * @param lastPose the lastPose to set
+   */
+  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
+
+  std::pair<frc::Pose3d, units::second_t> Update();
+
 private:
  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
  PoseStrategy strategy;
@@ -104,13 +144,44 @@ class RobotPoseEstimator {
  frc::Pose3d lastPose;
  frc::Pose3d referencePose;

-  std::pair<frc::Pose3d, units::millisecond_t> LowestAmbiguityStrategy();
+  /**
+   * Return the estimated position of the robot with the lowest position
+   * ambiguity from a List of pipeline results.
+   *
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> LowestAmbiguityStrategy();

-  std::pair<frc::Pose3d, units::millisecond_t> ClosestToCameraHeightStrategy();
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta height difference between the estimated and actual height of the
+   * camera.
+   *
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> ClosestToCameraHeightStrategy();

-  std::pair<frc::Pose3d, units::millisecond_t> ClosestToReferencePoseStrategy();
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta in the vector magnitude between it and the reference pose.
+   *
+   * @param referencePose reference pose to check vector magnitude difference
+   * against.
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> ClosestToReferencePoseStrategy();

-  std::pair<frc::Pose3d, units::millisecond_t> AverageBestTargetsStrategy();
+  /**
+   * Return the average of the best target poses using ambiguity as weight.
+
+   * @return the estimated position of the robot in the FCS and the estimated
+   timestamp of this
+   *     estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> AverageBestTargetsStrategy();
 };

 }  // namespace photonlib
--- a/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h
+++ b/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h
@@ -108,6 +108,14 @@ class SimVisionSystem {
        ->SetPose(target.targetPose.ToPose2d());
  }

+  /**
+   * Clears all sim vision targets.
+   * This is useful for switching alliances and needing to repopulate the sim
+   * targets. NOTE: Old targets will still show on the Field2d unless
+   * overwritten by new targets with the same ID
+   */
+  void ClearVisionTargets() { targetList.clear(); }
+
  /**
   * Adjust the camera position relative to the robot. Use this if your camera
   * is on a gimbal or turret or some other mobile platform.
@@ -185,6 +193,7 @@ class SimVisionSystem {
                                camToTargetTransform,
                                // TODO ambiguity
                                0.0,
+                                {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
                                {{0, 0}, {0, 0}, {0, 0}, {0, 0}}});
      }

--- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
+++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
@@ -45,12 +45,12 @@ import java.util.List;
 import java.util.Optional;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
-import org.photonvision.RobotPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;

-class RobotPoseEstimatorTest {
+class PhotonPoseEstimatorTest {
    static AprilTagFieldLayout aprilTags;

    @BeforeAll
@@ -62,24 +62,22 @@ class RobotPoseEstimatorTest {

        try {
            CombinedRuntimeLoader.loadLibraries(
-                    RobotPoseEstimatorTest.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
+                    PhotonPoseEstimatorTest.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
        } catch (IOException e) {
            // TODO Auto-generated catch block
            e.printStackTrace();
        }

-        List<AprilTag> atList = new ArrayList<AprilTag>(2);
-        atList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
-        atList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
-        var fl = Units.feetToMeters(54.0);
-        var fw = Units.feetToMeters(27.0);
-        aprilTags = new AprilTagFieldLayout(atList, fl, fw);
+        List<AprilTag> tagList = new ArrayList<AprilTag>(2);
+        tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
+        tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
+        double fieldLength = Units.feetToMeters(54.0);
+        double fieldWidth = Units.feetToMeters(27.0);
+        aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
    }

    @Test
    void testLowestAmbiguityStrategy() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
-
        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
        cameraOne.result =
                new PhotonPipelineResult(
@@ -122,12 +120,7 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(1, 2),
                                                new TargetCorner(3, 4),
                                                new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-        PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))),
                                new PhotonTrackedTarget(
                                        9.0,
                                        -2.0,
@@ -147,17 +140,16 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(3, 4),
                                                new TargetCorner(5, 6),
                                                new TargetCorner(7, 8)))));
+        cameraOne.result.setTimestampSeconds(11);

-        cameras.add(Pair.of(cameraOne, new Transform3d()));
-        cameras.add(Pair.of(cameraTwo, new Transform3d()));
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, new Transform3d());

-        RobotPoseEstimator estimator =
-                new RobotPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameras);
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().estimatedPose;

-        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.get().getFirst();
-
-        assertEquals(2, estimatedPose.get().getSecond());
+        assertEquals(11, estimatedPose.get().timestampSeconds);
        assertEquals(1, pose.getX(), .01);
        assertEquals(3, pose.getY(), .01);
        assertEquals(2, pose.getZ(), .01);
@@ -209,12 +201,7 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(1, 2),
                                                new TargetCorner(3, 4),
                                                new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-        PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))),
                                new PhotonTrackedTarget(
                                        9.0,
                                        -2.0,
@@ -235,16 +222,19 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(5, 6),
                                                new TargetCorner(7, 8)))));

-        cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 4), new Rotation3d())));
-        cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 2), new Rotation3d())));
+        cameraOne.result.setTimestampSeconds(4);

-        RobotPoseEstimator estimator =
-                new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, cameras);
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
+                        cameraOne,
+                        new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));

-        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.get().getFirst();
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().estimatedPose;

-        assertEquals(2, estimatedPose.get().getSecond());
+        assertEquals(4, estimatedPose.get().timestampSeconds);
        assertEquals(4, pose.getX(), .01);
        assertEquals(4, pose.getY(), .01);
        assertEquals(0, pose.getZ(), .01);
@@ -252,8 +242,6 @@ class RobotPoseEstimatorTest {

    @Test
    void closestToReferencePoseStrategy() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
-
        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
        cameraOne.result =
                new PhotonPipelineResult(
@@ -296,12 +284,7 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(1, 2),
                                                new TargetCorner(3, 4),
                                                new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-        PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))),
                                new PhotonTrackedTarget(
                                        9.0,
                                        -2.0,
@@ -321,18 +304,20 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(3, 4),
                                                new TargetCorner(5, 6),
                                                new TargetCorner(7, 8)))));
+        cameraOne.result.setTimestampSeconds(17);

-        cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
-        cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
-
-        RobotPoseEstimator estimator =
-                new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cameras);
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
+                        cameraOne,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
        estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));

-        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.get().getFirst();
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().estimatedPose;

-        assertEquals(4, estimatedPose.get().getSecond());
+        assertEquals(17, estimatedPose.get().timestampSeconds);
        assertEquals(1, pose.getX(), .01);
        assertEquals(1.1, pose.getY(), .01);
        assertEquals(.9, pose.getZ(), .01);
@@ -340,8 +325,6 @@ class RobotPoseEstimatorTest {

    @Test
    void closestToLastPose() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
-
        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
        cameraOne.result =
                new PhotonPipelineResult(
@@ -384,12 +367,7 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(1, 2),
                                                new TargetCorner(3, 4),
                                                new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-        PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))),
                                new PhotonTrackedTarget(
                                        9.0,
                                        -2.0,
@@ -410,16 +388,17 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(5, 6),
                                                new TargetCorner(7, 8)))));

-        cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
-        cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
-
-        RobotPoseEstimator estimator =
-                new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_LAST_POSE, cameras);
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.CLOSEST_TO_LAST_POSE,
+                        cameraOne,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));

        estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));

-        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.get().getFirst();
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().estimatedPose;

        cameraOne.result =
                new PhotonPipelineResult(
@@ -462,11 +441,7 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(1, 2),
                                                new TargetCorner(3, 4),
                                                new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))),
                                new PhotonTrackedTarget(
                                        9.0,
                                        -2.0,
@@ -486,11 +461,12 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(3, 4),
                                                new TargetCorner(5, 6),
                                                new TargetCorner(7, 8)))));
+        cameraOne.result.setTimestampSeconds(7);

        estimatedPose = estimator.update();
-        pose = estimatedPose.get().getFirst();
+        pose = estimatedPose.get().estimatedPose;

-        assertEquals(2, estimatedPose.get().getSecond());
+        assertEquals(7, estimatedPose.get().timestampSeconds);
        assertEquals(.9, pose.getX(), .01);
        assertEquals(1.1, pose.getY(), .01);
        assertEquals(1, pose.getZ(), .01);
@@ -542,12 +518,7 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(1, 2),
                                                new TargetCorner(3, 4),
                                                new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8))))); // 2 2 2 ambig .3
-        PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))), // 2 2 2 ambig .3
                                new PhotonTrackedTarget(
                                        9.0,
                                        -2.0,
@@ -567,16 +538,19 @@ class RobotPoseEstimatorTest {
                                                new TargetCorner(3, 4),
                                                new TargetCorner(5, 6),
                                                new TargetCorner(7, 8))))); // 3 3 3 ambig .4
+        cameraOne.result.setTimestampSeconds(20);

-        cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
-        cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.AVERAGE_BEST_TARGETS,
+                        cameraOne,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));

-        RobotPoseEstimator estimator =
-                new RobotPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS, cameras);
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().estimatedPose;

-        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.get().getFirst();
-        assertEquals(2.6885245901639347, estimatedPose.get().getSecond(), .01);
+        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
        assertEquals(2.15, pose.getX(), .01);
        assertEquals(2.15, pose.getY(), .01);
        assertEquals(2.15, pose.getZ(), .01);
--- a/photon-lib/src/test/native/cpp/PacketTest.cpp
+++ b/photon-lib/src/test/native/cpp/PacketTest.cpp
@@ -42,6 +42,7 @@ TEST(PacketTest, PhotonTrackedTarget) {
      frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                       frc::Rotation3d(1_rad, 2_rad, 3_rad)),
      -1,
+      {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
      {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}};

  photonlib::Packet p;
@@ -79,6 +80,7 @@ TEST(PacketTest, PhotonPipelineResult) {
          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
          -1,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
      photonlib::PhotonTrackedTarget{
          3.0,
@@ -91,6 +93,7 @@ TEST(PacketTest, PhotonPipelineResult) {
          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
          -1,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
           std::pair{7, 8}}}};

--- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
+++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
@@ -0,0 +1,312 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <map>
+#include <utility>
+#include <vector>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Rotation3d.h>
+#include <frc/geometry/Transform3d.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpi/SmallVector.h>
+
+#include "gtest/gtest.h"
+#include "photonlib/PhotonCamera.h"
+#include "photonlib/PhotonPipelineResult.h"
+#include "photonlib/PhotonPoseEstimator.h"
+#include "photonlib/PhotonTrackedTarget.h"
+
+static std::vector<frc::AprilTag> tags = {
+    {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                    frc::Rotation3d())},
+    {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                    frc::Rotation3d())}};
+
+static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
+
+static wpi::SmallVector<std::pair<double, double>, 4> corners{
+    std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
+static std::vector<std::pair<double, double>> detectedCorners{
+    std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
+
+TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 0,
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {2_ms, targets};
+  cameraOne.testResult.SetTimestamp(units::second_t(11));
+
+  photonlib::PhotonPoseEstimator estimator(
+      aprilTags, photonlib::LOWEST_AMBIGUITY, std::move(cameraOne), {});
+  auto estimatedPose = estimator.Update();
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .02);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
+  std::vector<frc::AprilTag> tags = {
+      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                      frc::Rotation3d())},
+      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                      frc::Rotation3d())},
+  };
+  auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft);
+
+  std::vector<std::pair<photonlib::PhotonCamera, frc::Transform3d>> cameras;
+
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
+
+  // ID 0 at 3,3,3
+  // ID 1 at 5,5,5
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {2_ms, targets};
+  cameraOne.testResult.SetTimestamp(17_s);
+
+  photonlib::PhotonPoseEstimator estimator(
+      aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
+      {{0_m, 0_m, 4_m}, {}});
+  auto estimatedPose = estimator.Update();
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .02);
+  EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(0, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {2_ms, targets};
+  cameraOne.testResult.SetTimestamp(units::second_t(17));
+
+  photonlib::PhotonPoseEstimator estimator(aprilTags,
+                                           photonlib::CLOSEST_TO_REFERENCE_POSE,
+                                           std::move(cameraOne), {});
+  estimator.SetReferencePose(
+      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
+  auto estimatedPose = estimator.Update();
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .01);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {2_ms, targets};
+  cameraOne.testResult.SetTimestamp(units::second_t(17));
+
+  photonlib::PhotonPoseEstimator estimator(
+      aprilTags, photonlib::CLOSEST_TO_LAST_POSE, std::move(cameraOne), {});
+  estimator.SetLastPose(
+      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
+  auto estimatedPose = estimator.Update();
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targetsThree{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 0,
+          frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  estimator.GetCamera().testResult = {2_ms, targetsThree};
+  estimator.GetCamera().testResult.SetTimestamp(units::second_t(7));
+
+  estimatedPose = estimator.Update();
+  pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .01);
+  EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 0,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {2_ms, targets};
+  cameraOne.testResult.SetTimestamp(units::second_t(15));
+
+  photonlib::PhotonPoseEstimator estimator(
+      aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
+  auto estimatedPose = estimator.Update();
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .01);
+  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
+}
--- a/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp
+++ b/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp
@@ -40,6 +40,11 @@
 #include "photonlib/PhotonTrackedTarget.h"
 #include "photonlib/RobotPoseEstimator.h"

+static wpi::SmallVector<std::pair<double, double>, 4> corners{
+    std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
+static std::vector<std::pair<double, double>> detectedCorners{
+    std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
+
 TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
  std::vector<frc::AprilTag> tags = {
      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
@@ -61,51 +66,36 @@ TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          0,
+          3.0, -4.0, 9.0, 4.0, 0,
          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          1,
+          3.0, -4.0, 9.1, 6.7, 1,
          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};

  cameraOne->test = true;
  cameraOne->testResult = {2_s, targets};
+  cameraOne->testResult.SetTimestamp(units::second_t(11));

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
      photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};

  cameraTwo->test = true;
  cameraTwo->testResult = {4_s, targetsTwo};
+  cameraTwo->testResult.SetTimestamp(units::second_t(units::second_t(16)));

  cameras.push_back(std::make_pair(
      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
@@ -119,7 +109,7 @@ TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
      estimator.Update();
  frc::Pose3d pose = estimatedPose.first;

-  EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .01);
+  EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
  EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
  EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
@@ -146,51 +136,36 @@ TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          1,
+          3.0, -4.0, 9.0, 4.0, 1,
          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          1,
+          3.0, -4.0, 9.1, 6.7, 1,
          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};

  cameraOne->test = true;
  cameraOne->testResult = {2_s, targets};
+  cameraOne->testResult.SetTimestamp(units::second_t(4));

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
      photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
          frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};

  cameraTwo->test = true;
  cameraTwo->testResult = {4_s, targetsTwo};
+  cameraOne->testResult.SetTimestamp(units::second_t(12));

  cameras.push_back(std::make_pair(
      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 4_m),
@@ -204,7 +179,7 @@ TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
      estimator.Update();
  frc::Pose3d pose = estimatedPose.first;

-  EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .01);
+  EXPECT_NEAR(12, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
  EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
  EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
  EXPECT_NEAR(4, units::unit_cast<double>(pose.Z()), .01);
@@ -231,51 +206,36 @@ TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          1,
+          3.0, -4.0, 9.0, 4.0, 1,
          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          1,
+          3.0, -4.0, 9.1, 6.7, 1,
          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};

  cameraOne->test = true;
  cameraOne->testResult = {2_s, targets};
+  cameraOne->testResult.SetTimestamp(units::second_t(4));

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
      photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
          frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};

  cameraTwo->test = true;
  cameraTwo->testResult = {4_s, targetsTwo};
+  cameraTwo->testResult.SetTimestamp(units::second_t(17));

  cameras.push_back(std::make_pair(
      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
@@ -291,7 +251,7 @@ TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
      estimator.Update();
  frc::Pose3d pose = estimatedPose.first;

-  EXPECT_NEAR(4, units::unit_cast<double>(estimatedPose.second), .01);
+  EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
  EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
  EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
@@ -317,48 +277,31 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          1,
+          3.0, -4.0, 9.0, 4.0, 1,
          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          1,
+          3.0, -4.0, 9.1, 6.7, 1,
          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};

  cameraOne->test = true;
  cameraOne->testResult = {2_s, targets};

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
      photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
          frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};

  cameraTwo->test = true;
  cameraTwo->testResult = {4_s, targetsTwo};
@@ -379,49 +322,34 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targetsThree{
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          1,
+          3.0, -4.0, 9.0, 4.0, 1,
          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          0,
+          3.0, -4.0, 9.1, 6.7, 0,
          frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};

  cameraOne->testResult = {2_s, targetsThree};
+  cameraOne->testResult.SetTimestamp(units::second_t(7));

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsFour{
      photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
          frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};

  cameraTwo->testResult = {4_s, targetsFour};
+  cameraTwo->testResult.SetTimestamp(units::second_t(13));

  std::vector<
      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -436,7 +364,8 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
  estimatedPose = estimator.Update();
  pose = estimatedPose.first;

-  EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .01);
+  EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
+              .01);
  EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
  EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
  EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
@@ -462,51 +391,36 @@ TEST(RobotPoseEstimatorTest, AverageBestPoses) {

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          0,
+          3.0, -4.0, 9.0, 4.0, 0,
          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
      photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          1,
+          3.0, -4.0, 9.1, 6.7, 1,
          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};

  cameraOne->test = true;
  cameraOne->testResult = {2_s, targets};
+  cameraOne->testResult.SetTimestamp(units::second_t(10));

  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
      photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};

  cameraTwo->test = true;
  cameraTwo->testResult = {4_s, targetsTwo};
+  cameraTwo->testResult.SetTimestamp(units::second_t(20));

  cameras.push_back(std::make_pair(
      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
@@ -520,8 +434,8 @@ TEST(RobotPoseEstimatorTest, AverageBestPoses) {
      estimator.Update();
  frc::Pose3d pose = estimatedPose.first;

-  EXPECT_NEAR(2.6885245901639347,
-              units::unit_cast<double>(estimatedPose.second), .01);
+  EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
+              .01);
  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
--- a/photon-server/src/main/java/org/photonvision/Main.java
+++ b/photon-server/src/main/java/org/photonvision/Main.java
@@ -169,24 +169,6 @@ public class Main {
    private static void addTestModeSources() {
        ConfigManager.getInstance().load();

-        var camConfApril =
-                ConfigManager.getInstance().getConfig().getCameraConfigurations().get("Apriltag");
-        if (camConfApril == null) {
-            camConfApril =
-                    new CameraConfiguration("Apriltag", TestUtils.getTestModeApriltagPath().toString());
-            camConfApril.FOV = TestUtils.WPI2019Image.FOV;
-            camConfApril.calibrations.add(TestUtils.get2019LifeCamCoeffs(true));
-
-            var pipeline2019 = new AprilTagPipelineSettings();
-            pipeline2019.pipelineNickname = "Robots";
-            pipeline2019.outputShowMultipleTargets = true;
-            pipeline2019.inputShouldShow = true;
-
-            var psList2019 = new ArrayList<CVPipelineSettings>();
-            psList2019.add(pipeline2019);
-            camConfApril.pipelineSettings = psList2019;
-        }
-
        var camConf2019 =
                ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2019");
        if (camConf2019 == null) {
@@ -245,6 +227,32 @@ public class Main {
            camConf2022.pipelineSettings = psList2022;
        }

+        CameraConfiguration camConf2023 =
+                ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2023");
+        if (camConf2023 == null) {
+            camConf2023 =
+                    new CameraConfiguration(
+                            "WPI2023",
+                            TestUtils.getResourcesFolderPath(true)
+                                    .resolve("testimages")
+                                    .resolve(TestUtils.WPI2023Apriltags.k383_60_Angle2.path)
+                                    .toString());
+
+            camConf2023.FOV = TestUtils.WPI2023Apriltags.FOV;
+            camConf2023.calibrations.add(TestUtils.get2023LifeCamCoeffs(true));
+
+            var pipeline2023 = new AprilTagPipelineSettings();
+            var path_split = Path.of(camConf2023.path).getFileName().toString();
+            pipeline2023.pipelineNickname = path_split.replace(".png", "");
+            pipeline2023.targetModel = TargetModel.k6in_16h5;
+            pipeline2023.inputShouldShow = true;
+            pipeline2023.solvePNPEnabled = true;
+
+            var psList2023 = new ArrayList<CVPipelineSettings>();
+            psList2023.add(pipeline2023);
+            camConf2023.pipelineSettings = psList2023;
+        }
+
        // Colored shape testing
        var camConfShape =
                ConfigManager.getInstance().getConfig().getCameraConfigurations().get("Shape");
@@ -269,13 +277,13 @@ public class Main {

        var collectedSources = new ArrayList<VisionSource>();

-        var fvsApril = new FileVisionSource(camConfApril);
        var fvsShape = new FileVisionSource(camConfShape);
        var fvs2019 = new FileVisionSource(camConf2019);
        var fvs2020 = new FileVisionSource(camConf2020);
        var fvs2022 = new FileVisionSource(camConf2022);
+        var fvs2023 = new FileVisionSource(camConf2023);

-        collectedSources.add(fvsApril);
+        collectedSources.add(fvs2023);
        collectedSources.add(fvs2022);
        collectedSources.add(fvsShape);
        collectedSources.add(fvs2020);
--- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
+++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
@@ -18,6 +18,7 @@
 package org.photonvision.server;

 import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.JsonNode;
 import com.fasterxml.jackson.databind.ObjectMapper;
 import io.javalin.http.Context;
 import java.io.File;
@@ -33,6 +34,9 @@ import java.util.Map;
 import org.apache.commons.io.FileUtils;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.configuration.NetworkConfig;
+import org.photonvision.common.dataflow.DataChangeDestination;
+import org.photonvision.common.dataflow.DataChangeService;
+import org.photonvision.common.dataflow.events.IncomingWebSocketEvent;
 import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
 import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.hardware.Platform;
@@ -42,6 +46,7 @@ import org.photonvision.common.networking.NetworkManager;
 import org.photonvision.common.util.ShellExec;
 import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.common.util.file.ProgramDirectoryUtilities;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.processes.VisionModuleManager;
 import org.photonvision.vision.target.TargetModel;

@@ -274,6 +279,44 @@ public class RequestHandler {
        }
    }

+    public static void importCalibrationFromCalibdb(Context ctx) {
+        var file = ctx.body();
+
+        if (file != null) {
+            // check if it's a JSON file
+            // Load using Jackson
+            try {
+                ObjectMapper mapper = new ObjectMapper();
+                JsonNode actualObj = mapper.readTree(file);
+
+                int cameraIndex = actualObj.get("cameraIndex").asInt();
+                String filename = actualObj.get("filename").asText();
+                var payload = mapper.readTree(actualObj.get("payload").asText());
+
+                var coeffs = CameraCalibrationCoefficients.parseFromCalibdbJson(payload);
+
+                var uploadCalibrationEvent =
+                        new IncomingWebSocketEvent<CameraCalibrationCoefficients>(
+                                DataChangeDestination.DCD_ACTIVEMODULE,
+                                "calibrationUploaded",
+                                coeffs,
+                                (Integer) cameraIndex,
+                                null);
+                DataChangeService.getInstance().publishEvent(uploadCalibrationEvent);
+
+                ctx.status(200);
+                logger.info("Calibration added!");
+            } catch (Exception e) {
+                logger.warn("Could not parse cal metaJSON!");
+                e.printStackTrace();
+                return;
+            }
+        } else {
+            ctx.status(500);
+            return;
+        }
+    }
+
    public static void setCameraNickname(Context ctx) {
        try {
            var data = kObjectMapper.readValue(ctx.body(), HashMap.class);
@@ -304,7 +347,8 @@ public class RequestHandler {

    public static void sendMetrics(Context ctx) {
        HardwareManager.getInstance().publishMetrics();
-        // TimedTaskManager.getInstance().addOneShotTask(() -> RoborioFinder.getInstance().findRios(),
+        // TimedTaskManager.getInstance().addOneShotTask(() ->
+        // RoborioFinder.getInstance().findRios(),
        // 0);
        ctx.status(200);
    }
--- a/photon-server/src/main/java/org/photonvision/server/Server.java
+++ b/photon-server/src/main/java/org/photonvision/server/Server.java
@@ -93,6 +93,7 @@ public class Server {
        app.post("api/vision/pnpModel", RequestHandler::uploadPnpModel);
        app.post("api/sendMetrics", RequestHandler::sendMetrics);
        app.post("api/setCameraNickname", RequestHandler::setCameraNickname);
+        app.post("api/calibration/import", RequestHandler::importCalibrationFromCalibdb);

        app.start(port);
    }
--- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java
+++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java
@@ -75,9 +75,11 @@ public class NTTopicSet {
        pipelineIndexPublisher = pipelineIndexTopic.publish();
        pipelineIndexSubscriber = pipelineIndexTopic.subscribe(0);

-        driverModeEntry = subTable.getBooleanTopic("driverMode");
-        driverModePublisher = driverModeEntry.publish();
-        driverModeSubscriber = driverModeEntry.subscribe(false);
+        driverModePublisher = subTable.getBooleanTopic("driverMode").publish();
+        driverModeSubscriber = subTable.getBooleanTopic("driverModeRequest").subscribe(false);
+
+        // Fun little hack to make the request show up
+        driverModeSubscriber.getTopic().publish().setDefault(false);

        latencyMillisEntry = subTable.getDoubleTopic("latencyMillis").publish();
        hasTargetEntry = subTable.getBooleanTopic("hasTarget").publish();
--- a/photonlib-cpp-examples/aimandrange/build.gradle
+++ b/photonlib-cpp-examples/aimandrange/build.gradle
@@ -1,7 +1,7 @@
 plugins {
    id "cpp"
    id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"

    id "com.dorongold.task-tree" version "2.1.0"
 }
--- a/photonlib-cpp-examples/aimattarget/build.gradle
+++ b/photonlib-cpp-examples/aimattarget/build.gradle
@@ -1,7 +1,7 @@
 plugins {
    id "cpp"
    id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"

    id "com.dorongold.task-tree" version "2.1.0"
 }
--- a/photonlib-cpp-examples/getinrange/build.gradle
+++ b/photonlib-cpp-examples/getinrange/build.gradle
@@ -1,7 +1,7 @@
 plugins {
    id "cpp"
    id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"

    id "com.dorongold.task-tree" version "2.1.0"
 }
--- a/photonlib-java-examples/aimandrange/build.gradle
+++ b/photonlib-java-examples/aimandrange/build.gradle
@@ -1,6 +1,6 @@
 plugins {
    id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"
 }

 sourceCompatibility = JavaVersion.VERSION_11
--- a/photonlib-java-examples/aimattarget/build.gradle
+++ b/photonlib-java-examples/aimattarget/build.gradle
@@ -1,6 +1,6 @@
 plugins {
    id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"
 }

 sourceCompatibility = JavaVersion.VERSION_11
--- a/photonlib-java-examples/apriltagExample/build.gradle
+++ b/photonlib-java-examples/apriltagExample/build.gradle
@@ -1,6 +1,6 @@
 plugins {
    id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"
 }

 sourceCompatibility = JavaVersion.VERSION_11
--- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java
+++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java
@@ -24,7 +24,6 @@

 package frc.robot;

-import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
@@ -49,6 +48,8 @@ import edu.wpi.first.wpilibj.simulation.EncoderSim;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.DriveTrainConstants;
+import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;

 /** Represents a differential drive style drivetrain. */
 public class Drivetrain {
@@ -183,13 +184,14 @@ public class Drivetrain {
        // Also apply vision measurements. We use 0.3 seconds in the past as an example
        // -- on
        // a real robot, this must be calculated based either on latency or timestamps.
-        Pair<Pose2d, Double> result =
+        Optional<EstimatedRobotPose> result =
                pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition());
-        var camPose = result.getFirst();
-        var camPoseObsTime = result.getSecond();
-        if (camPose != null) {
-            m_poseEstimator.addVisionMeasurement(camPose, camPoseObsTime);
-            m_fieldSim.getObject("Cam Est Pos").setPose(camPose);
+
+        if (result.isPresent()) {
+            EstimatedRobotPose camPose = result.get();
+            m_poseEstimator.addVisionMeasurement(
+                    camPose.estimatedPose.toPose2d(), camPose.timestampSeconds);
+            m_fieldSim.getObject("Cam Est Pos").setPose(camPose.estimatedPose.toPose2d());
        } else {
            // move it way off the screen to make it disappear
            m_fieldSim.getObject("Cam Est Pos").setPose(new Pose2d(-100, -100, new Rotation2d()));
--- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java
+++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java
@@ -26,23 +26,21 @@ package frc.robot;

 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.wpilibj.Timer;
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.VisionConstants;
 import java.util.ArrayList;
 import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
-import org.photonvision.RobotPoseEstimator;
-import org.photonvision.RobotPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;

 public class PhotonCameraWrapper {
    public PhotonCamera photonCamera;
-    public RobotPoseEstimator robotPoseEstimator;
+    public PhotonPoseEstimator photonPoseEstimator;

    public PhotonCameraWrapper() {
        // Set up a test arena of two apriltags at the center of each driver station set
@@ -73,14 +71,10 @@ public class PhotonCameraWrapper {
                                .cameraName); // Change the name of your camera here to whatever it is in the
        // PhotonVision UI.

-        // ... Add other cameras here
-
-        // Assemble the list of cameras & mount locations
-        var camList = new ArrayList<Pair<PhotonCamera, Transform3d>>();
-        camList.add(new Pair<PhotonCamera, Transform3d>(photonCamera, VisionConstants.robotToCam));
-
-        robotPoseEstimator =
-                new RobotPoseEstimator(atfl, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, camList);
+        // Create pose estimator
+        photonPoseEstimator =
+                new PhotonPoseEstimator(
+                        atfl, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, photonCamera, VisionConstants.robotToCam);
    }

    /**
@@ -88,16 +82,8 @@ public class PhotonCameraWrapper {
     * @return A pair of the fused camera observations to a single Pose2d on the field, and the time
     *     of the observation. Assumes a planar field and the robot is always firmly on the ground
     */
-    public Pair<Pose2d, Double> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
-        robotPoseEstimator.setReferencePose(prevEstimatedRobotPose);
-
-        double currentTime = Timer.getFPGATimestamp();
-        Optional<Pair<Pose3d, Double>> result = robotPoseEstimator.update();
-        if (result.isPresent()) {
-            return new Pair<Pose2d, Double>(
-                    result.get().getFirst().toPose2d(), currentTime - result.get().getSecond());
-        } else {
-            return new Pair<Pose2d, Double>(null, 0.0);
-        }
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
+        photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
+        return photonPoseEstimator.update();
    }
 }
--- a/photonlib-java-examples/getinrange/build.gradle
+++ b/photonlib-java-examples/getinrange/build.gradle
@@ -1,6 +1,6 @@
 plugins {
    id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"
 }

 sourceCompatibility = JavaVersion.VERSION_11
--- a/photonlib-java-examples/simaimandrange/build.gradle
+++ b/photonlib-java-examples/simaimandrange/build.gradle
@@ -1,6 +1,6 @@
 plugins {
    id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"
 }

 sourceCompatibility = JavaVersion.VERSION_11
--- a/photonlib-java-examples/simposeest/build.gradle
+++ b/photonlib-java-examples/simposeest/build.gradle
@@ -1,6 +1,6 @@
 plugins {
    id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"
 }

 sourceCompatibility = JavaVersion.VERSION_11
--- a/scripts/install.sh
+++ b/scripts/install.sh
@@ -1,5 +1,9 @@
 #!/bin/bash

+package_is_installed(){
+    dpkg-query -W -f='${Status}' "$1" 2>/dev/null | grep -q "ok installed"
+}
+
 if [ "$(id -u)" != "0" ]; then
   echo "This script must be run as root" 1>&2
   exit 1
@@ -50,13 +54,24 @@ else
 fi

 echo "Installing the JDK..."
-if [ $(dpkg-query -W -f='${Status}' openjdk-11-jdk-headless 2>/dev/null | grep -c "ok installed") -eq 0 ];
+if ! package_is_installed openjdk-11-jdk-headless
 then
   apt-get update
   apt-get install --yes openjdk-11-jdk-headless
 fi
 echo "JDK installation complete."

+if [ "$ARCH" == "aarch64" ]
+then
+    if package_is_installed libopencv-core4.5
+    then
+        echo "libopencv-core4.5 already installed"
+    else
+        # libphotonlibcamera.so on raspberry pi has dep on libopencv_core
+        echo "Installing libopencv-core4.5 on aarch64"
+        apt-get install --yes libopencv-core4.5
+    fi
+fi

 echo "Downloading latest stable release of PhotonVision..."
 mkdir -p /opt/photonvision
--- a/test-resources/calibration/lifecam_1280.json
+++ b/test-resources/calibration/lifecam_1280.json
@@ -0,0 +1,31 @@
+{
+  "resolution": {
+    "width": 1280.0,
+    "height": 720.0
+  },
+  "cameraIntrinsics": {
+    "rows": 3,
+    "cols": 3,
+    "type": 6,
+    "data": [
+      1142.3413236140077, 0.0, 621.3842013099301, 0.0, 1139.922141261303,
+      349.8976310349802, 0.0, 0.0, 1.0
+    ]
+  },
+  "cameraExtrinsics": {
+    "rows": 1,
+    "cols": 5,
+    "type": 6,
+    "data": [
+      0.1621109548975541, -1.269454756369703, 0.0027858284948980416,
+      -2.8060142397060727e-4, 2.502076853514765
+    ]
+  },
+  "perViewErrors": [
+    0.5169799553578814, 0.8652021086925443, 0.2641133839198221,
+    0.19142021439095572, 0.30802391826048936, 0.43007079636212353,
+    0.2678347488666545, 1.1019555236060878, 0.4021403723943656,
+    0.3109898316125448, 0.4183600115684474, 0.4975122345222207
+  ],
+  "standardDeviation": 0.25485508324006756
+}
--- a/test-resources/testimages/2023/AprilTags/162_36_Angle.png
+++ b/test-resources/testimages/2023/AprilTags/162_36_Angle.png
--- a/test-resources/testimages/2023/AprilTags/162_36_Straight.png
+++ b/test-resources/testimages/2023/AprilTags/162_36_Straight.png
--- a/test-resources/testimages/2023/AprilTags/383_60_Angle1.png
+++ b/test-resources/testimages/2023/AprilTags/383_60_Angle1.png
--- a/test-resources/testimages/2023/AprilTags/383_60_Angle2.png
+++ b/test-resources/testimages/2023/AprilTags/383_60_Angle2.png
--- a/test-resources/testimages/2023/AprilTags/383_60_Straight.png
+++ b/test-resources/testimages/2023/AprilTags/383_60_Straight.png
--- a/test-resources/testimages/2023/AprilTags/ReadMe.txt
+++ b/test-resources/testimages/2023/AprilTags/ReadMe.txt
@@ -0,0 +1,6 @@
+All images are taken using a Microsoft Lifecam HD-3000
+
+Measurements in the filename are approximate coordinates in inches, X followed by Y. The coordinate system is
+the same as the Apriltag layout coordinates with the origin at the corner of the Blue Alliance wall, + Y moving across the
+length of the field towards the Red Alliance Wall, +X moving across the with of the field towads the Substations (see the
+Official Drawing package for a visual)
Author	SHA1	Message	Date
Matt	e58c27caa2	Bump LL image to fix NetworkManager (#780 )	2023-01-31 06:57:45 -06:00
Matt	f6e3c9b3ee	Fix desync between web UI and NT (#778 ) Actually calls VisionModule::setPipeline when changing pipelines (needed to change video modes)	2023-01-29 23:30:34 -05:00
Matt	88ed2ebf51	Add PhotonVersion to sources/headers zip (#777 ) * Add PhotonVersion to sources/headers zip * Update publish.gradle	2023-01-29 23:30:22 -05:00
David Vo	5f39123bde	photon-lib: Fix C++ sources publish classifier (#765 ) The canonical classifier is sources, not source.	2023-01-27 10:52:14 -05:00
Matt	37a7d378fd	Fix publish type in photoncamera (#760 )	2023-01-22 10:56:41 -05:00
Matt	811fef1212	Bump pi image versions (#747 )	2023-01-18 16:31:42 -05:00
Matt	d0162b0ed0	Switch network management to networkmanager on Linux (#738 ) * Switch network management to networkmanager * Run style * Fix command formatting * Add curst Pi 5 second sleep * Run formatter * Also bring up/down on other linux * Switch to nmcli down/up * Remove sleep in nmcli down/up * Address review	2023-01-18 16:31:14 -05:00
Matt	9d6997180d	Add calibdb upload button (#735 ) * Add calibdb upload * Fix distortion coefficients size	2023-01-18 16:29:58 -05:00
smoser-frc	a985c6cf3a	Fix #748 - add libopencv-core4.5 for aarch64 systems. (#749 ) * Add and use a function in install.sh to determine if package is installed. Move the "is a package installed" code into a function. * Install libopencv-core4.5 on aarch64, which is likely raspberry pi. The libphotonvision.so on Raspberry pi depends on libopencv-core4.5. The code here installs that package on all aarch64 systems, as there was not an obvious way to install on only Raspberry pi systems. Fixes #748. Co-authored-by: Scott Moser <smoser@brickies.net>	2023-01-18 09:25:10 -05:00
Sriman Achanta	167a4661ca	[NFC] Update RobotPoseEstimator documentation (#740 ) * update documentation * add suggested changes * rename April Tag to AprilTag * Update RobotPoseEstimator.java Co-authored-by: Mohammad Durrani <46766905+mdurrani808@users.noreply.github.com>	2023-01-17 20:34:21 -05:00
Matt	a16ac4af57	Bump to wpilib 2023.2.1 (#741 )	2023-01-15 10:12:25 -05:00
Matt	d9f99f9c9b	Add calibration decimate dropdown (#739 ) * Increase resized size to 640 * Add calibration decimation dropdown * Update Calibrate3dPipeTest.java * Only allow decimation down to >=320x240 * Update CamerasView.vue	2023-01-14 19:23:14 -06:00
Andrew Gasser	357d8a518a	Return named type from PhotonPoseEstimator (#734 ) Adds PhotonPoseEstimator class, and deprecates RobotPoseEstimator	2023-01-14 10:06:15 -05:00
Matt	073714f0bc	[AprilTags] Reduce default iterations to 40 (#726 ) Co-authored-by: Mohammad Durrani <46766905+mdurrani808@users.noreply.github.com>	2023-01-11 16:32:31 -05:00
Nick Hadley	39f6ab8805	Add method to clear sim targets (#733 ) Closes #731	2023-01-11 12:33:19 -05:00
Mohammad Durrani	5c66785095	Delete EigenCore.h (#732 )	2023-01-10 21:37:22 -08:00
Matt	53c67a07e4	[photonlib] Only link to apriltag_shared (#730 )	2023-01-10 10:09:24 -05:00
Matt	7c985e3a84	Remove force istestmode in Main (#723 )	2023-01-10 09:13:59 -05:00
Jack	80e16ece87	Add hostname to camerapublisher mjpeg stream (#722 ) Closes #721	2023-01-09 13:11:49 -05:00
Matt	86b9d4b037	Add 2023 pics to test mode (#720 )	2023-01-07 20:48:21 -05:00