Fix driver mode pub/sub key mixup (#810)

* Fix non-checked optional in mulit-tag

* Fix fallback return

* Run format

.github/workflows/main.yml

@@ -11,6 +11,7 @@ on:
       - 'v*'
   pull_request:
     branches: [ master ]
+  merge_group:
 
 jobs:
   # This job builds the client (web view).
@@ -393,11 +394,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.3_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_suffix: limelight2
+              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 }}"

.gitignore

@@ -153,3 +153,5 @@ photon-server/src/main/resources/nativelibraries/apriltag/*
 
 photonlib-java-examples/*/vendordeps/*
 photonlib-cpp-examples/*/vendordeps/*
+
+*/networktables.json

build.gradle

@@ -26,7 +26,7 @@ allprojects {
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2023.1.1"
+    wpilibVersion = "2023.3.2"
     opencvVersion = "4.6.0-4"
     joglVersion = "2.4.0-rc-20200307"
     pubVersion = versionString

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!",
+            };
+            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;

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;
                 }
             )

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")

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -89,7 +89,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
             ts.pipelineIndexPublisher.set(setIndex);
             // TODO: Log
         }
-        logger.debug("Successfully set pipeline index to " + newIndex);
+        logger.debug("Set pipeline index to " + newIndex);
     }
 
     private void onDriverModeChange(NetworkTableEvent entryNotification) {
@@ -102,7 +102,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
         }
 
         driverModeConsumer.accept(newDriverMode);
-        logger.debug("Successfully set driver mode to " + newDriverMode);
+        logger.debug("Set driver mode to " + newDriverMode);
     }
 
     private void removeEntries() {
@@ -119,7 +119,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
 
         pipelineIndexListener =
                 new NTDataChangeListener(
-                        ts.subTable.getInstance(), ts.pipelineIndexSubscriber, this::onPipelineIndexChange);
+                        ts.subTable.getInstance(), ts.pipelineIndexRequestSub, this::onPipelineIndexChange);
 
         driverModeListener =
                 new NTDataChangeListener(
@@ -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());
 
@@ -180,6 +180,15 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
             ts.bestTargetPosY.set(0);
         }
 
+        var fsp = result.inputAndOutputFrame.frameStaticProperties;
+        if (fsp.cameraCalibration != null) {
+            ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr());
+            ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getExtrinsicsArr());
+        } else {
+            ts.cameraIntrinsicsPublisher.accept(new double[] {});
+            ts.cameraDistortionPublisher.accept(new double[] {});
+        }
+
         ts.heartbeatPublisher.set(heartbeatCounter++);
 
         // TODO...nt4... is this needed?

photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java

@@ -17,7 +17,8 @@
 
 package org.photonvision.common.hardware;
 
-import edu.wpi.first.networktables.IntegerEntry;
+import edu.wpi.first.networktables.IntegerPublisher;
+import edu.wpi.first.networktables.IntegerSubscriber;
 import java.io.IOException;
 import org.photonvision.common.ProgramStatus;
 import org.photonvision.common.configuration.ConfigManager;
@@ -47,7 +48,9 @@ public class HardwareManager {
     private final StatusLED statusLED;
 
     @SuppressWarnings("FieldCanBeLocal")
-    private final IntegerEntry ledModeEntry;
+    private IntegerSubscriber ledModeRequest;
+
+    private IntegerPublisher ledModeState;
 
     @SuppressWarnings({"FieldCanBeLocal", "unused"})
     private final NTDataChangeListener ledModeListener;
@@ -71,6 +74,15 @@ public class HardwareManager {
         this.metricsManager = new MetricsManager();
         this.metricsManager.setConfig(hardwareConfig);
 
+        ledModeRequest =
+                NetworkTablesManager.getInstance()
+                        .kRootTable
+                        .getIntegerTopic("ledModeRequest")
+                        .subscribe(-1);
+        ledModeState =
+                NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledModeState").publish();
+        ledModeState.set(VisionLEDMode.kDefault.value);
+
         CustomGPIO.setConfig(hardwareConfig);
 
         if (Platform.isRaspberryPi()) {
@@ -92,17 +104,15 @@ public class HardwareManager {
                                 hardwareConfig.ledPins,
                                 hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(0) : 0,
                                 hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(1) : 100,
-                                pigpioSocket);
+                                pigpioSocket,
+                                ledModeState::set);
 
-        ledModeEntry =
-                NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledMode").getEntry(0);
-        ledModeEntry.set(VisionLEDMode.kDefault.value);
         ledModeListener =
                 visionLED == null
                         ? null
                         : new NTDataChangeListener(
                                 NetworkTablesManager.getInstance().kRootTable.getInstance(),
-                                ledModeEntry,
+                                ledModeRequest,
                                 visionLED::onLedModeChange);
 
         Runtime.getRuntime().addShutdownHook(new Thread(this::onJvmExit));

photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java

@@ -21,6 +21,7 @@ import edu.wpi.first.networktables.NetworkTableEvent;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
 import org.photonvision.common.hardware.GPIO.CustomGPIO;
 import org.photonvision.common.hardware.GPIO.GPIOBase;
 import org.photonvision.common.hardware.GPIO.pi.PigpioException;
@@ -45,11 +46,18 @@ public class VisionLED {
 
     private int mappedBrightnessPercentage;
 
+    private Consumer<Integer> modeConsumer;
+
     public VisionLED(
-            List<Integer> ledPins, int brightnessMin, int brightnessMax, PigpioSocket pigpioSocket) {
+            List<Integer> ledPins,
+            int brightnessMin,
+            int brightnessMax,
+            PigpioSocket pigpioSocket,
+            Consumer<Integer> visionLEDmode) {
         this.brightnessMin = brightnessMin;
         this.brightnessMax = brightnessMax;
         this.pigpioSocket = pigpioSocket;
+        this.modeConsumer = visionLEDmode;
         this.ledPins = ledPins.stream().mapToInt(i -> i).toArray();
         ledPins.forEach(
                 pin -> {
@@ -123,7 +131,8 @@ public class VisionLED {
     }
 
     void onLedModeChange(NetworkTableEvent entryNotification) {
-        var newLedModeRaw = (int) entryNotification.valueData.value.getDouble();
+        var newLedModeRaw = (int) entryNotification.valueData.value.getInteger();
+        logger.debug("Got LED mode " + newLedModeRaw);
         if (newLedModeRaw != currentLedMode.value) {
             VisionLEDMode newLedMode;
             switch (newLedModeRaw) {
@@ -145,6 +154,8 @@ public class VisionLED {
                     break;
             }
             setInternal(newLedMode, true);
+
+            if (modeConsumer != null) modeConsumer.accept(newLedMode.value);
         }
     }
 

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);
                     }

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();
+    }
 }

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);
+    }
 }

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);
     }

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;
@@ -43,6 +44,10 @@ public class CameraCalibrationCoefficients implements Releasable {
     @JsonProperty("standardDeviation")
     public final double standardDeviation;
 
+    @JsonIgnore private final double[] intrinsicsArr = new double[9];
+
+    @JsonIgnore private final double[] extrinsicsArr = new double[5];
+
     @JsonCreator
     public CameraCalibrationCoefficients(
             @JsonProperty("resolution") Size resolution,
@@ -55,6 +60,10 @@ public class CameraCalibrationCoefficients implements Releasable {
         this.distCoeffs = distCoeffs;
         this.perViewErrors = perViewErrors;
         this.standardDeviation = standardDeviation;
+
+        // do this once so gets are quick
+        getCameraIntrinsicsMat().get(0, 0, intrinsicsArr);
+        getDistCoeffsMat().get(0, 0, extrinsicsArr);
     }
 
     @JsonIgnore
@@ -67,6 +76,16 @@ public class CameraCalibrationCoefficients implements Releasable {
         return distCoeffs.getAsMatOfDouble();
     }
 
+    @JsonIgnore
+    public double[] getIntrinsicsArr() {
+        return intrinsicsArr;
+    }
+
+    @JsonIgnore
+    public double[] getExtrinsicsArr() {
+        return extrinsicsArr;
+    }
+
     @JsonIgnore
     public double[] getPerViewErrors() {
         return perViewErrors;
@@ -82,4 +101,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);
+    }
 }

photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java

@@ -23,6 +23,7 @@ import java.util.Arrays;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfDouble;
+import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.vision.opencv.Releasable;
 
 public class JsonMat implements Releasable {
@@ -106,4 +107,9 @@ public class JsonMat implements Releasable {
     public void release() {
         getAsMat().release();
     }
+
+    public Packet populatePacket(Packet packet) {
+        packet.encode(this.data);
+        return packet;
+    }
 }

photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java

@@ -61,10 +61,12 @@ public class FrameStaticProperties {
 
         imageArea = this.imageWidth * this.imageHeight;
 
+        // Todo -- if we have calibration, use it's center point?
         centerX = ((double) this.imageWidth / 2) - 0.5;
         centerY = ((double) this.imageHeight / 2) - 0.5;
         centerPoint = new Point(centerX, centerY);
 
+        // TODO if we have calibration use it here instead
         // pinhole model calculations
         DoubleCouple horizVertViews =
                 calculateHorizontalVerticalFoV(this.fov, this.imageWidth, this.imageHeight);

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;
         }
     }
 }

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;
 

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);
         }
 

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;
     }
 }

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);
@@ -363,7 +363,7 @@ public class VisionModule {
 
         if (ret != null) {
             logger.debug("Saving calibration...");
-            visionSource.getSettables().getConfiguration().addCalibration(ret);
+            visionSource.getSettables().addCalibration(ret);
         } else {
             logger.error("Calibration failed...");
         }
@@ -371,15 +371,15 @@ public class VisionModule {
         return ret;
     }
 
-    void setPipeline(int index) {
+    boolean setPipeline(int index) {
         logger.info("Setting pipeline to " + index);
         logger.info("Pipeline name: " + pipelineManager.getPipelineNickname(index));
         pipelineManager.setIndex(index);
         var pipelineSettings = pipelineManager.getPipelineSettings(index);
 
         if (pipelineSettings == null) {
-            logger.error("Config for index " + index + " was null!");
-            return;
+            logger.error("Config for index " + index + " was null! Not changing pipelines");
+            return false;
         }
 
         visionSource.getSettables().setVideoModeInternal(pipelineSettings.cameraVideoModeIndex);
@@ -422,6 +422,8 @@ public class VisionModule {
 
         visionSource.getSettables().getConfiguration().currentPipelineIndex =
                 pipelineManager.getCurrentPipelineIndex();
+
+        return true;
     }
 
     private boolean camShouldControlLEDs() {
@@ -586,4 +588,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();
+    }
 }

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;

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

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);
     }

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()) {

photon-lib/build.gradle

@@ -22,6 +22,14 @@ targetCompatibility = JavaVersion.VERSION_11
 wpilibTools.deps.wpilibVersion = wpilibVersion
 println("Buidling for wpilib ${wpilibTools.deps.wpilibVersion}")
 
+// From wpilib shared/config.gradle:
+// NativeUtils adds the following OpenCV warning suppression for Linux, but not
+// for macOS
+// https://github.com/opencv/opencv/issues/20269
+nativeUtils.platformConfigs.osxuniversal.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion")
+nativeUtils.platformConfigs.linuxathena.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion")
+// nativeUtils.platformConfigs.linuxx64.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion")
+
 // Apply Java configuration
 dependencies {
     implementation project(":photon-targeting")
@@ -34,6 +42,11 @@ dependencies {
     implementation wpilibTools.deps.wpilibJava("wpilibj")
     implementation wpilibTools.deps.wpilibJava("apriltag")
 
+    testImplementation wpilibTools.deps.wpilibJava("cscore")
+    implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-java:$opencvVersion"
+    implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-jni:$opencvVersion:$jniPlatform"
+    implementation "org.ejml:ejml-simple:0.41"
+
     // Junit
     testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
     testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
@@ -65,7 +78,8 @@ model {
                 }
             }
             nativeUtils.useRequiredLibrary(it, "wpilib_shared")
-            nativeUtils.useRequiredLibrary(it, "vision_shared")
+            nativeUtils.useRequiredLibrary(it, "apriltag_shared")
+            nativeUtils.useRequiredLibrary(it, "opencv_shared")
         }
     }
     testSuites {
@@ -80,8 +94,9 @@ model {
             }
 
             nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
-            nativeUtils.useRequiredLibrary(it, "vision_shared")
+            nativeUtils.useRequiredLibrary(it, "apriltag_shared")
             nativeUtils.useRequiredLibrary(it, "googletest_static")
+            nativeUtils.useRequiredLibrary(it, "opencv_shared")
         }
     }
 }
@@ -123,6 +138,10 @@ task writeCurrentVersion {
 
 build.dependsOn writeCurrentVersion
 
+tasks.withType(Javadoc) {
+    options.encoding = 'UTF-8'
+}
+
 apply from: "publish.gradle"
 
 def testNativeConfigName = 'wpilibTestNatives'
@@ -143,3 +162,4 @@ testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
 testNativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
 testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
 testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
+testNativeConfig.dependencies.add wpilibTools.deps.cscore()

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

photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java

@@ -0,0 +1,54 @@
+/*
+ * 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.math.geometry.Pose3d;
+import java.util.List;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+/** An estimated pose based on pipeline result */
+public class EstimatedRobotPose {
+    /** The estimated pose */
+    public final Pose3d estimatedPose;
+
+    /** The estimated time the frame used to derive the robot pose was taken */
+    public final double timestampSeconds;
+
+    /** A list of the targets used to compute this pose */
+    public final List<PhotonTrackedTarget> targetsUsed;
+
+    /**
+     * Constructs an EstimatedRobotPose
+     *
+     * @param estimatedPose estimated pose
+     * @param timestampSeconds timestamp of the estimate
+     */
+    public EstimatedRobotPose(
+            Pose3d estimatedPose, double timestampSeconds, List<PhotonTrackedTarget> targetsUsed) {
+        this.estimatedPose = estimatedPose;
+        this.timestampSeconds = timestampSeconds;
+        this.targetsUsed = targetsUsed;
+    }
+}

photon-lib/src/main/java/org/photonvision/PhotonCamera.java

@@ -24,12 +24,17 @@
 
 package org.photonvision;
 
-import edu.wpi.first.networktables.BooleanEntry;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.*;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleArrayPublisher;
+import edu.wpi.first.networktables.DoubleArraySubscriber;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.IntegerEntry;
+import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.IntegerSubscriber;
 import edu.wpi.first.networktables.MultiSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
@@ -39,6 +44,7 @@ import edu.wpi.first.networktables.RawSubscriber;
 import edu.wpi.first.networktables.StringSubscriber;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import java.util.Optional;
 import java.util.Set;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.hardware.VisionLEDMode;
@@ -48,9 +54,8 @@ import org.photonvision.targeting.PhotonPipelineResult;
 public class PhotonCamera {
     static final String kTableName = "photonvision";
 
-    protected final NetworkTable rootTable;
+    protected final NetworkTable cameraTable;
     RawSubscriber rawBytesEntry;
-    BooleanEntry driverModeEntry;
     BooleanPublisher driverModePublisher;
     BooleanSubscriber driverModeSubscriber;
     DoublePublisher latencyMillisEntry;
@@ -62,12 +67,14 @@ public class PhotonCamera {
     DoublePublisher targetSkewEntry;
     StringSubscriber versionEntry;
     IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
-    IntegerEntry pipelineIndexEntry, ledModeEntry;
+    IntegerPublisher pipelineIndexRequest, ledModeRequest;
+    IntegerSubscriber pipelineIndexState, ledModeState;
     IntegerSubscriber heartbeatEntry;
+    private DoubleArraySubscriber cameraIntrinsicsSubscriber;
+    private DoubleArraySubscriber cameraDistortionSubscriber;
 
     public void close() {
         rawBytesEntry.close();
-        driverModeEntry.close();
         driverModePublisher.close();
         driverModeSubscriber.close();
         latencyMillisEntry.close();
@@ -80,8 +87,13 @@ public class PhotonCamera {
         versionEntry.close();
         inputSaveImgEntry.close();
         outputSaveImgEntry.close();
-        pipelineIndexEntry.close();
-        ledModeEntry.close();
+        pipelineIndexRequest.close();
+        pipelineIndexState.close();
+        ledModeRequest.close();
+        ledModeState.close();
+        pipelineIndexRequest.close();
+        cameraIntrinsicsSubscriber.close();
+        cameraDistortionSubscriber.close();
     }
 
     private final String path;
@@ -113,21 +125,29 @@ public class PhotonCamera {
      */
     public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         name = cameraName;
-        var mainTable = instance.getTable(kTableName);
-        this.rootTable = mainTable.getSubTable(cameraName);
-        path = rootTable.getPath();
+        var photonvision_root_table = instance.getTable(kTableName);
+        this.cameraTable = photonvision_root_table.getSubTable(cameraName);
+        path = cameraTable.getPath();
         rawBytesEntry =
-                rootTable
+                cameraTable
                         .getRawTopic("rawBytes")
                         .subscribe(
                                 "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
-        driverModeEntry = rootTable.getBooleanTopic("driverMode").getEntry(false);
-        inputSaveImgEntry = rootTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
-        outputSaveImgEntry = rootTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
-        pipelineIndexEntry = rootTable.getIntegerTopic("pipelineIndex").getEntry(0);
-        heartbeatEntry = rootTable.getIntegerTopic("heartbeat").subscribe(-1);
-        ledModeEntry = mainTable.getIntegerTopic("ledMode").getEntry(-1);
-        versionEntry = mainTable.getStringTopic("version").subscribe("");
+        driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
+        driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
+        inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
+        outputSaveImgEntry = cameraTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
+        pipelineIndexRequest = cameraTable.getIntegerTopic("pipelineIndexRequest").publish();
+        pipelineIndexState = cameraTable.getIntegerTopic("pipelineIndexState").subscribe(0);
+        heartbeatEntry = cameraTable.getIntegerTopic("heartbeat").subscribe(-1);
+        cameraIntrinsicsSubscriber =
+                cameraTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null);
+        cameraDistortionSubscriber =
+                cameraTable.getDoubleArrayTopic("cameraDistortion").subscribe(null);
+
+        ledModeRequest = photonvision_root_table.getIntegerTopic("ledModeRequest").publish();
+        ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1);
+        versionEntry = photonvision_root_table.getStringTopic("version").subscribe("");
 
         m_topicNameSubscriber =
                 new MultiSubscriber(
@@ -179,7 +199,7 @@ public class PhotonCamera {
      * @return Whether the camera is in driver mode.
      */
     public boolean getDriverMode() {
-        return driverModeEntry.get(false);
+        return driverModeSubscriber.get();
     }
 
     /**
@@ -188,7 +208,7 @@ public class PhotonCamera {
      * @param driverMode Whether to set driver mode.
      */
     public void setDriverMode(boolean driverMode) {
-        driverModeEntry.set(driverMode);
+        driverModePublisher.set(driverMode);
     }
 
     /**
@@ -217,7 +237,7 @@ public class PhotonCamera {
      * @return The active pipeline index.
      */
     public int getPipelineIndex() {
-        return (int) pipelineIndexEntry.get(0);
+        return (int) pipelineIndexState.get(0);
     }
 
     /**
@@ -226,7 +246,7 @@ public class PhotonCamera {
      * @param index The active pipeline index.
      */
     public void setPipelineIndex(int index) {
-        pipelineIndexEntry.set(index);
+        pipelineIndexRequest.set(index);
     }
 
     /**
@@ -235,7 +255,7 @@ public class PhotonCamera {
      * @return The current LED mode.
      */
     public VisionLEDMode getLEDMode() {
-        int value = (int) ledModeEntry.get(-1);
+        int value = (int) ledModeState.get(-1);
         switch (value) {
             case 0:
                 return VisionLEDMode.kOff;
@@ -255,7 +275,7 @@ public class PhotonCamera {
      * @param led The mode to set to.
      */
     public void setLED(VisionLEDMode led) {
-        ledModeEntry.set(led.value);
+        ledModeRequest.set(led.value);
     }
 
     /**
@@ -300,6 +320,20 @@ public class PhotonCamera {
         return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
     }
 
+    public Optional<Matrix<N3, N3>> getCameraMatrix() {
+        var cameraMatrix = cameraIntrinsicsSubscriber.get();
+        if (cameraMatrix != null && cameraMatrix.length == 9) {
+            return Optional.of(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(cameraMatrix));
+        } else return Optional.empty();
+    }
+
+    public Optional<Matrix<N5, N1>> getDistCoeffs() {
+        var distCoeffs = cameraDistortionSubscriber.get();
+        if (distCoeffs != null && distCoeffs.length == 5) {
+            return Optional.of(new MatBuilder<>(Nat.N5(), Nat.N1()).fill(distCoeffs));
+        } else return Optional.empty();
+    }
+
     private void verifyVersion() {
         if (!VERSION_CHECK_ENABLED) return;
 
@@ -309,7 +343,7 @@ public class PhotonCamera {
         // Heartbeat entry is assumed to always be present. If it's not present, we
         // assume that a camera with that name was never connected in the first place.
         if (!heartbeatEntry.exists()) {
-            Set<String> cameraNames = rootTable.getInstance().getTable(kTableName).getSubTables();
+            Set<String> cameraNames = cameraTable.getInstance().getTable(kTableName).getSubTables();
             if (cameraNames.isEmpty()) {
                 DriverStation.reportError(
                         "Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!",

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -0,0 +1,651 @@
+/*
+ * 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.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.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.estimation.VisionEstimation;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
+
+/**
+ * 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,
+
+        /** Return the average of the best target poses using ambiguity as weight. */
+        AVERAGE_BEST_TARGETS,
+
+        /** Use all visible tags to compute a single pose estimate.. */
+        MULTI_TAG_PNP
+    }
+
+    private AprilTagFieldLayout fieldTags;
+    private PoseStrategy primaryStrategy;
+    private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
+    private final PhotonCamera camera;
+    private Transform3d robotToCamera;
+
+    private Pose3d lastPose;
+    private Pose3d referencePose;
+    protected double poseCacheTimestampSeconds = -1;
+    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 PhotonCamera
+     * @param robotToCamera Transform3d from the center of the robot to the camera mount position (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.primaryStrategy = strategy;
+        this.camera = camera;
+        this.robotToCamera = robotToCamera;
+    }
+
+    /** Invalidates the pose cache. */
+    private void invalidatePoseCache() {
+        poseCacheTimestampSeconds = -1;
+    }
+
+    private void checkUpdate(Object oldObj, Object newObj) {
+        if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) {
+            invalidatePoseCache();
+        }
+    }
+
+    /**
+     * 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) {
+        checkUpdate(this.fieldTags, fieldTags);
+        this.fieldTags = fieldTags;
+    }
+
+    /**
+     * Get the Position Estimation Strategy being used by the Position Estimator.
+     *
+     * @return the strategy
+     */
+    public PoseStrategy getPrimaryStrategy() {
+        return primaryStrategy;
+    }
+
+    /**
+     * Set the Position Estimation Strategy used by the Position Estimator.
+     *
+     * @param strategy the strategy to set
+     */
+    public void setPrimaryStrategy(PoseStrategy strategy) {
+        checkUpdate(this.primaryStrategy, strategy);
+        this.primaryStrategy = strategy;
+    }
+
+    /**
+     * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must
+     * NOT be MULTI_TAG_PNP
+     *
+     * @param strategy the strategy to set
+     */
+    public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
+        checkUpdate(this.multiTagFallbackStrategy, strategy);
+        if (strategy == PoseStrategy.MULTI_TAG_PNP) {
+            DriverStation.reportWarning(
+                    "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", null);
+            strategy = PoseStrategy.LOWEST_AMBIGUITY;
+        }
+        this.multiTagFallbackStrategy = 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) {
+        checkUpdate(this.referencePose, 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) {
+        setReferencePose(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));
+    }
+
+    /** @return The current transform from the center of the robot to the camera mount position */
+    public Transform3d getRobotToCameraTransform() {
+        return robotToCamera;
+    }
+
+    /**
+     * Useful for pan and tilt mechanisms and such.
+     *
+     * @param robotToCamera The current transform from the center of the robot to the camera mount
+     *     position
+     */
+    public void setRobotToCameraTransform(Transform3d robotToCamera) {
+        this.robotToCamera = robotToCamera;
+    }
+
+    /**
+     * 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, the timestamp, and targets 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();
+
+        return update(cameraResult);
+    }
+
+    /**
+     * Updates the estimated position of the robot. Returns empty if there are no cameras set or no
+     * targets were found from the cameras.
+     *
+     * @param cameraResult The latest pipeline result from the camera
+     * @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(PhotonPipelineResult cameraResult) {
+        // Time in the past -- give up, since the following if expects times > 0
+        if (cameraResult.getTimestampSeconds() < 0) {
+            return Optional.empty();
+        }
+
+        // If the pose cache timestamp was set, and the result is from the same timestamp, return an
+        // empty result
+        if (poseCacheTimestampSeconds > 0
+                && Math.abs(poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()) < 1e-6) {
+            return Optional.empty();
+        }
+
+        // Remember the timestamp of the current result used
+        poseCacheTimestampSeconds = cameraResult.getTimestampSeconds();
+
+        // If no targets seen, trivial case -- return empty result
+        if (!cameraResult.hasTargets()) {
+            return Optional.empty();
+        }
+
+        return update(cameraResult, this.primaryStrategy);
+    }
+
+    private Optional<EstimatedRobotPose> update(
+            PhotonPipelineResult cameraResult, PoseStrategy strat) {
+        Optional<EstimatedRobotPose> estimatedPose;
+        switch (strat) {
+            case LOWEST_AMBIGUITY:
+                estimatedPose = lowestAmbiguityStrategy(cameraResult);
+                break;
+            case CLOSEST_TO_CAMERA_HEIGHT:
+                estimatedPose = closestToCameraHeightStrategy(cameraResult);
+                break;
+            case CLOSEST_TO_REFERENCE_POSE:
+                estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose);
+                break;
+            case CLOSEST_TO_LAST_POSE:
+                setReferencePose(lastPose);
+                estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose);
+                break;
+            case AVERAGE_BEST_TARGETS:
+                estimatedPose = averageBestTargetsStrategy(cameraResult);
+                break;
+            case MULTI_TAG_PNP:
+                estimatedPose = multiTagPNPStrategy(cameraResult);
+                break;
+            default:
+                DriverStation.reportError(
+                        "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false);
+                return Optional.empty();
+        }
+
+        if (estimatedPose.isEmpty()) {
+            lastPose = null;
+        }
+
+        return estimatedPose;
+    }
+
+    private Optional<EstimatedRobotPose> multiTagPNPStrategy(PhotonPipelineResult result) {
+        // Arrays we need declared up front
+        var visCorners = new ArrayList<TargetCorner>();
+        var knownVisTags = new ArrayList<AprilTag>();
+        var fieldToCams = new ArrayList<Pose3d>();
+        var fieldToCamsAlt = new ArrayList<Pose3d>();
+
+        if (result.getTargets().size() < 2) {
+            // Run fallback strategy instead
+            return update(result, this.multiTagFallbackStrategy);
+        }
+
+        for (var target : result.getTargets()) {
+            visCorners.addAll(target.getDetectedCorners());
+
+            var tagPoseOpt = fieldTags.getTagPose(target.getFiducialId());
+            if (tagPoseOpt.isEmpty()) {
+                reportFiducialPoseError(target.getFiducialId());
+                continue;
+            }
+
+            var tagPose = tagPoseOpt.get();
+
+            // actual layout poses of visible tags -- not exposed, so have to recreate
+            knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose));
+
+            fieldToCams.add(tagPose.transformBy(target.getBestCameraToTarget().inverse()));
+            fieldToCamsAlt.add(tagPose.transformBy(target.getAlternateCameraToTarget().inverse()));
+        }
+
+        var cameraMatrixOpt = camera.getCameraMatrix();
+        var distCoeffsOpt = camera.getDistCoeffs();
+        boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
+
+        // multi-target solvePNP
+        if (hasCalibData) {
+            var cameraMatrix = cameraMatrixOpt.get();
+            var distCoeffs = distCoeffsOpt.get();
+            var pnpResults =
+                    VisionEstimation.estimateCamPosePNP(cameraMatrix, distCoeffs, visCorners, knownVisTags);
+            var best =
+                    new Pose3d()
+                            .plus(pnpResults.best) // field-to-camera
+                            .plus(robotToCamera.inverse()); // field-to-robot
+            // var alt = new Pose3d()
+            // .plus(pnpResults.alt) // field-to-camera
+            // .plus(robotToCamera.inverse()); // field-to-robot
+
+            return Optional.of(
+                    new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets()));
+        } else {
+            // TODO fallback strategy? Should we just always do solvePNP?
+            return Optional.empty();
+        }
+    }
+
+    /**
+     * 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(),
+                        result.getTargets()));
+    }
+
+    /**
+     * 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(),
+                                result.getTargets());
+            }
+
+            if (bestTransformDelta < smallestHeightDifference) {
+                smallestHeightDifference = bestTransformDelta;
+                closestHeightTarget =
+                        new EstimatedRobotPose(
+                                targetPosition
+                                        .get()
+                                        .transformBy(target.getBestCameraToTarget().inverse())
+                                        .transformBy(robotToCamera.inverse()),
+                                result.getTimestampSeconds(),
+                                result.getTargets());
+            }
+        }
+
+        // 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(), result.getTargets());
+            }
+            if (bestDifference < smallestPoseDelta) {
+                smallestPoseDelta = bestDifference;
+                lowestDeltaPose =
+                        new EstimatedRobotPose(
+                                bestTransformPosition, result.getTimestampSeconds(), result.getTargets());
+            }
+        }
+        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(),
+                                result.getTargets()));
+            }
+
+            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(), result.getTargets()));
+    }
+
+    /**
+     * 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);
+        }
+    }
+}

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);
         }
     }

photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java

@@ -24,6 +24,10 @@
 
 package org.photonvision;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import java.util.Arrays;
 import java.util.List;
@@ -52,6 +56,28 @@ public class SimPhotonCamera {
         ts.updateEntries();
     }
 
+    /**
+     * Publishes the camera intrinsics matrix. The matrix should be in the form: spotless:off
+     * fx  0   cx
+     * 0   fy  cy
+     * 0   0   1
+     * @param cameraMatrix The cam matrix
+     * spotless:on
+     */
+    public void setCameraIntrinsicsMat(Matrix<N3, N3> cameraMatrix) {
+        ts.cameraIntrinsicsPublisher.set(cameraMatrix.getData());
+    }
+
+    /**
+     * Publishes the camera distortion matrix. The matrix should be in the form [k1 k2 p1 p2 k3]. See
+     * more: https://docs.opencv.org/3.4/d4/d94/tutorial_camera_calibration.html
+     *
+     * @param distortionMat The distortion mat
+     */
+    public void setCameraDistortionMat(Matrix<N5, N1> distortionMat) {
+        ts.cameraDistortionPublisher.set(distortionMat.getData());
+    }
+
     /**
      * Constructs a Simulated PhotonCamera from the name of the camera.
      *

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.

photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java

@@ -0,0 +1,64 @@
+/*
+ * 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.estimation;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
+
+/** Holds various helper geometries describing the relation between camera and target. */
+public class CameraTargetRelation {
+    public final Pose3d camPose;
+    public final Transform3d camToTarg;
+    public final double camToTargDist;
+    public final double camToTargDistXY;
+    public final Rotation2d camToTargYaw;
+    public final Rotation2d camToTargPitch;
+    /** Angle from the camera's relative x-axis */
+    public final Rotation2d camToTargAngle;
+
+    public final Transform3d targToCam;
+    public final Rotation2d targToCamYaw;
+    public final Rotation2d targToCamPitch;
+    /** Angle from the target's relative x-axis */
+    public final Rotation2d targToCamAngle;
+
+    public CameraTargetRelation(Pose3d cameraPose, Pose3d targetPose) {
+        this.camPose = cameraPose;
+        camToTarg = new Transform3d(cameraPose, targetPose);
+        camToTargDist = camToTarg.getTranslation().getNorm();
+        camToTargDistXY =
+                Math.hypot(camToTarg.getTranslation().getX(), camToTarg.getTranslation().getY());
+        camToTargYaw = new Rotation2d(camToTarg.getX(), camToTarg.getY());
+        camToTargPitch = new Rotation2d(camToTargDistXY, -camToTarg.getZ());
+        camToTargAngle =
+                new Rotation2d(Math.hypot(camToTargYaw.getRadians(), camToTargPitch.getRadians()));
+        targToCam = new Transform3d(targetPose, cameraPose);
+        targToCamYaw = new Rotation2d(targToCam.getX(), targToCam.getY());
+        targToCamPitch = new Rotation2d(camToTargDistXY, -targToCam.getZ());
+        targToCamAngle =
+                new Rotation2d(Math.hypot(targToCamYaw.getRadians(), targToCamPitch.getRadians()));
+    }
+}

photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java

@@ -0,0 +1,518 @@
+/*
+ * 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.estimation;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.CoordinateSystem;
+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.math.numbers.*;
+import edu.wpi.first.util.RuntimeLoader;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import org.ejml.simple.SimpleMatrix;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.Core;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.MatOfInt;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point;
+import org.opencv.core.Point3;
+import org.opencv.core.Rect;
+import org.opencv.core.RotatedRect;
+import org.opencv.imgproc.Imgproc;
+import org.photonvision.targeting.TargetCorner;
+
+public final class OpenCVHelp {
+    static {
+        try {
+            var loader =
+                    new RuntimeLoader<>(
+                            Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
+            loader.loadLibrary();
+        } catch (Exception e) {
+            throw new RuntimeException("Failed to load native libraries!", e);
+        }
+    }
+
+    public static MatOfDouble matrixToMat(SimpleMatrix matrix) {
+        var mat = new Mat(matrix.numRows(), matrix.numCols(), CvType.CV_64F);
+        mat.put(0, 0, matrix.getDDRM().getData());
+        var wrappedMat = new MatOfDouble();
+        mat.convertTo(wrappedMat, CvType.CV_64F);
+        mat.release();
+        return wrappedMat;
+    }
+
+    public static Matrix<Num, Num> matToMatrix(Mat mat) {
+        double[] data = new double[(int) mat.total() * mat.channels()];
+        var doubleMat = new Mat(mat.rows(), mat.cols(), CvType.CV_64F);
+        mat.convertTo(doubleMat, CvType.CV_64F);
+        doubleMat.get(0, 0, data);
+        return new Matrix<>(new SimpleMatrix(mat.rows(), mat.cols(), true, data));
+    }
+
+    /**
+     * Creates a new {@link MatOfPoint3f} with these 3d translations. The opencv tvec is a vector with
+     * three elements representing {x, y, z} in the EDN coordinate system.
+     *
+     * @param translations The translations to convert into a MatOfPoint3f
+     */
+    public static MatOfPoint3f translationToTvec(Translation3d... translations) {
+        Point3[] points = new Point3[translations.length];
+        for (int i = 0; i < translations.length; i++) {
+            var trl =
+                    CoordinateSystem.convert(translations[i], CoordinateSystem.NWU(), CoordinateSystem.EDN());
+            points[i] = new Point3(trl.getX(), trl.getY(), trl.getZ());
+        }
+        return new MatOfPoint3f(points);
+    }
+    /**
+     * Returns a new 3d translation from this {@link Mat}. The opencv tvec is a vector with three
+     * elements representing {x, y, z} in the EDN coordinate system.
+     *
+     * @param tvecInput The tvec to create a Translation3d from
+     */
+    public static Translation3d tvecToTranslation(Mat tvecInput) {
+        float[] data = new float[3];
+        var wrapped = new Mat(tvecInput.rows(), tvecInput.cols(), CvType.CV_32F);
+        tvecInput.convertTo(wrapped, CvType.CV_32F);
+        wrapped.get(0, 0, data);
+        wrapped.release();
+        return CoordinateSystem.convert(
+                new Translation3d(data[0], data[1], data[2]),
+                CoordinateSystem.EDN(),
+                CoordinateSystem.NWU());
+    }
+
+    /**
+     * Creates a new {@link MatOfPoint3f} with this 3d rotation. The opencv rvec Mat is a vector with
+     * three elements representing the axis scaled by the angle in the EDN coordinate system. (angle =
+     * norm, and axis = rvec / norm)
+     *
+     * @param rotation The rotation to convert into a MatOfPoint3f
+     */
+    public static MatOfPoint3f rotationToRvec(Rotation3d rotation) {
+        rotation = rotationNWUtoEDN(rotation);
+        return new MatOfPoint3f(new Point3(rotation.getQuaternion().toRotationVector().getData()));
+    }
+    /**
+     * Returns a 3d rotation from this {@link Mat}. The opencv rvec Mat is a vector with three
+     * elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm,
+     * and axis = rvec / norm)
+     *
+     * @param rvecInput The rvec to create a Rotation3d from
+     */
+    public static Rotation3d rvecToRotation(Mat rvecInput) {
+        float[] data = new float[3];
+        var wrapped = new Mat(rvecInput.rows(), rvecInput.cols(), CvType.CV_32F);
+        rvecInput.convertTo(wrapped, CvType.CV_32F);
+        wrapped.get(0, 0, data);
+        wrapped.release();
+        Vector<N3> axis = new Vector<>(Nat.N3());
+        axis.set(0, 0, data[0]);
+        axis.set(1, 0, data[1]);
+        axis.set(2, 0, data[2]);
+        return rotationEDNtoNWU(new Rotation3d(axis.div(axis.norm()), axis.norm()));
+    }
+
+    public static TargetCorner averageCorner(List<TargetCorner> corners) {
+        if (corners == null || corners.size() == 0) return null;
+
+        var pointMat = targetCornersToMat(corners);
+        Core.reduce(pointMat, pointMat, 0, Core.REDUCE_AVG);
+        var avgPt = matToTargetCorners(pointMat)[0];
+        pointMat.release();
+        return avgPt;
+    }
+
+    public static MatOfPoint2f targetCornersToMat(List<TargetCorner> corners) {
+        return targetCornersToMat(corners.toArray(TargetCorner[]::new));
+    }
+
+    public static MatOfPoint2f targetCornersToMat(TargetCorner... corners) {
+        var points = new Point[corners.length];
+        for (int i = 0; i < corners.length; i++) {
+            points[i] = new Point(corners[i].x, corners[i].y);
+        }
+        return new MatOfPoint2f(points);
+    }
+
+    public static TargetCorner[] pointsToTargetCorners(Point... points) {
+        var corners = new TargetCorner[points.length];
+        for (int i = 0; i < points.length; i++) {
+            corners[i] = new TargetCorner(points[i].x, points[i].y);
+        }
+        return corners;
+    }
+
+    public static TargetCorner[] matToTargetCorners(MatOfPoint2f matInput) {
+        var corners = new TargetCorner[(int) matInput.total()];
+        float[] data = new float[(int) matInput.total() * matInput.channels()];
+        matInput.get(0, 0, data);
+        for (int i = 0; i < corners.length; i++) {
+            corners[i] = new TargetCorner(data[0 + 2 * i], data[1 + 2 * i]);
+        }
+        return corners;
+    }
+
+    /**
+     * Reorders the list, optionally indexing backwards and wrapping around to the last element after
+     * the first, and shifting all indices in the direction of indexing.
+     *
+     * <p>e.g.
+     *
+     * <p>({1,2,3}, false, 1) == {2,3,1}
+     *
+     * <p>({1,2,3}, true, 0) == {1,3,2}
+     *
+     * <p>({1,2,3}, true, 1) == {3,2,1}
+     *
+     * @param <T> Element type
+     * @param elements
+     * @param backwards If indexing should happen in reverse (0, size-1, size-2, ...)
+     * @param shiftStart How much the inital index should be shifted (instead of starting at index 0,
+     *     start at shiftStart, negated if backwards)
+     * @return Reordered list
+     */
+    public static <T> List<T> reorderCircular(List<T> elements, boolean backwards, int shiftStart) {
+        int size = elements.size();
+        int dir = backwards ? -1 : 1;
+        var reordered = new ArrayList<>(elements);
+        for (int i = 0; i < size; i++) {
+            int index = (i * dir + shiftStart * dir) % size;
+            if (index < 0) index = size + index;
+            reordered.set(i, elements.get(index));
+        }
+        return reordered;
+    }
+
+    /**
+     * Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN,
+     * this would be XYZ {0, -1, 0} in NWU.
+     */
+    private static Rotation3d rotationEDNtoNWU(Rotation3d rot) {
+        return CoordinateSystem.convert(
+                        new Rotation3d(), CoordinateSystem.NWU(), CoordinateSystem.EDN())
+                .plus(CoordinateSystem.convert(rot, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
+    }
+    /**
+     * Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN,
+     * this would be XYZ {0, -1, 0} in NWU.
+     */
+    private static Rotation3d rotationNWUtoEDN(Rotation3d rot) {
+        return CoordinateSystem.convert(
+                        new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU())
+                .plus(CoordinateSystem.convert(rot, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
+    }
+
+    /**
+     * Project object points from the 3d world into the 2d camera image. The camera
+     * properties(intrinsics, distortion) determine the results of this projection.
+     *
+     * @param cameraMatrix the camera intrinsics matrix in standard opencv form
+     * @param distCoeffs the camera distortion matrix in standard opencv form
+     * @param camPose The current camera pose in the 3d world
+     * @param objectTranslations The 3d points to be projected
+     * @return The 2d points in pixels which correspond to the image of the 3d points on the camera
+     */
+    public static List<TargetCorner> projectPoints(
+            Matrix<N3, N3> cameraMatrix,
+            Matrix<N5, N1> distCoeffs,
+            Pose3d camPose,
+            List<Translation3d> objectTranslations) {
+        // translate to opencv classes
+        var objectPoints = translationToTvec(objectTranslations.toArray(new Translation3d[0]));
+        // opencv rvec/tvec describe a change in basis from world to camera
+        var basisChange = RotTrlTransform3d.makeRelativeTo(camPose);
+        var rvec = rotationToRvec(basisChange.getRotation());
+        var tvec = translationToTvec(basisChange.getTranslation());
+        var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
+        var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
+        var imagePoints = new MatOfPoint2f();
+        // project to 2d
+        Calib3d.projectPoints(objectPoints, rvec, tvec, cameraMatrixMat, distCoeffsMat, imagePoints);
+
+        // turn 2d point Mat into TargetCorners
+        var corners = matToTargetCorners(imagePoints);
+
+        // release our Mats from native memory
+        objectPoints.release();
+        rvec.release();
+        tvec.release();
+        cameraMatrixMat.release();
+        distCoeffsMat.release();
+        imagePoints.release();
+
+        return Arrays.asList(corners);
+    }
+
+    /**
+     * Undistort 2d image points using a given camera's intrinsics and distortion.
+     *
+     * <p>2d image points from projectPoints(CameraProperties, Pose3d, List) projectPoints} will
+     * naturally be distorted, so this operation is important if the image points need to be directly
+     * used (e.g. 2d yaw/pitch).
+     *
+     * @param cameraMatrix the camera intrinsics matrix in standard opencv form
+     * @param distCoeffs the camera distortion matrix in standard opencv form
+     * @param corners The distorted image points
+     * @return The undistorted image points
+     */
+    public static List<TargetCorner> undistortPoints(
+            Matrix<N3, N3> cameraMatrix, Matrix<N5, N1> distCoeffs, List<TargetCorner> corners) {
+        var points_in = targetCornersToMat(corners);
+        var points_out = new MatOfPoint2f();
+        var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
+        var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
+
+        Calib3d.undistortImagePoints(points_in, points_out, cameraMatrixMat, distCoeffsMat);
+        var corners_out = matToTargetCorners(points_out);
+
+        points_in.release();
+        points_out.release();
+        cameraMatrixMat.release();
+        distCoeffsMat.release();
+
+        return Arrays.asList(corners_out);
+    }
+
+    /**
+     * Gets the (upright) rectangle which bounds this contour.
+     *
+     * <p>Note that rectangle size and position are stored with ints and do not have sub-pixel
+     * accuracy.
+     *
+     * @param corners The corners/points to be bounded
+     * @return Rectangle bounding the given corners
+     */
+    public static Rect getBoundingRect(List<TargetCorner> corners) {
+        var corn = targetCornersToMat(corners);
+        var rect = Imgproc.boundingRect(corn);
+        corn.release();
+        return rect;
+    }
+    /**
+     * Gets the rotated rectangle with minimum area which bounds this contour.
+     *
+     * <p>Note that rectangle size and position are stored with doubles and have sub-pixel accuracy.
+     *
+     * @param corners The corners/points to be bounded
+     * @return Rotated rectangle bounding the given corners
+     */
+    public static RotatedRect getMinAreaRect(List<TargetCorner> corners) {
+        var corn = targetCornersToMat(corners);
+        var rect = Imgproc.minAreaRect(corn);
+        corn.release();
+        return rect;
+    }
+    /**
+     * Get the area in pixels of this target's contour. It's important to note that this may be
+     * different from the area of the bounding rectangle around the contour.
+     *
+     * @param corners The corners defining this contour
+     * @return Area in pixels (units of corner x/y)
+     */
+    public static double getContourAreaPx(List<TargetCorner> corners) {
+        var temp = targetCornersToMat(corners);
+        var corn = new MatOfPoint(temp.toArray());
+        temp.release();
+
+        // outputHull gives us indices (of corn) that make a convex hull contour
+        var outputHull = new MatOfInt();
+        Imgproc.convexHull(corn, outputHull);
+        int[] indices = outputHull.toArray();
+        outputHull.release();
+        var tempPoints = corn.toArray();
+        var points = tempPoints.clone();
+        for (int i = 0; i < indices.length; i++) {
+            points[i] = tempPoints[indices[i]];
+        }
+        corn.fromArray(points);
+        // calculate area of the (convex hull) contour
+        double area = Imgproc.contourArea(corn);
+        corn.release();
+        return area;
+    }
+
+    /**
+     * Finds the transformation(s) that map the camera's pose to the target pose. The camera's pose
+     * relative to the target is determined by the supplied 3d points of the target's model and their
+     * associated 2d points imaged by the camera.
+     *
+     * <p>For planar targets, there may be an alternate solution which is plausible given the 2d image
+     * points. This has an associated "ambiguity" which describes the ratio of reprojection error
+     * between the "best" and "alternate" solution.
+     *
+     * <p>This method is intended for use with individual AprilTags, and will not work unless 4 points
+     * are provided.
+     *
+     * @param cameraMatrix the camera intrinsics matrix in standard opencv form
+     * @param distCoeffs the camera distortion matrix in standard opencv form
+     * @param modelTrls The translations of the object corners. These should have the object pose as
+     *     their origin. These must come in a specific, pose-relative order (in NWU):
+     *     <ul>
+     *       <li>Point 0: [0, -squareLength / 2, squareLength / 2]
+     *       <li>Point 1: [0, squareLength / 2, squareLength / 2]
+     *       <li>Point 2: [0, squareLength / 2, -squareLength / 2]
+     *       <li>Point 3: [0, -squareLength / 2, -squareLength / 2]
+     *     </ul>
+     *
+     * @param imageCorners The projection of these 3d object points into the 2d camera image. The
+     *     order should match the given object point translations.
+     * @return The resulting transformation that maps the camera pose to the target pose and the
+     *     ambiguity if an alternate solution is available.
+     */
+    public static PNPResults solvePNP_SQUARE(
+            Matrix<N3, N3> cameraMatrix,
+            Matrix<N5, N1> distCoeffs,
+            List<Translation3d> modelTrls,
+            List<TargetCorner> imageCorners) {
+        // IPPE_SQUARE expects our corners in a specific order
+        modelTrls = reorderCircular(modelTrls, true, -1);
+        imageCorners = reorderCircular(imageCorners, true, -1);
+        // translate to opencv classes
+        var objectPoints = translationToTvec(modelTrls.toArray(new Translation3d[0]));
+        var imagePoints = targetCornersToMat(imageCorners);
+        var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
+        var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
+        var rvecs = new ArrayList<Mat>();
+        var tvecs = new ArrayList<Mat>();
+        var rvec = Mat.zeros(3, 1, CvType.CV_32F);
+        var tvec = Mat.zeros(3, 1, CvType.CV_32F);
+        var reprojectionError = new Mat();
+        // calc rvecs/tvecs and associated reprojection error from image points
+        Calib3d.solvePnPGeneric(
+                objectPoints,
+                imagePoints,
+                cameraMatrixMat,
+                distCoeffsMat,
+                rvecs,
+                tvecs,
+                false,
+                Calib3d.SOLVEPNP_IPPE_SQUARE,
+                rvec,
+                tvec,
+                reprojectionError);
+
+        float[] errors = new float[2];
+        reprojectionError.get(0, 0, errors);
+        // convert to wpilib coordinates
+        var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
+
+        Transform3d alt = null;
+        if (tvecs.size() > 1) {
+            alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1)));
+        }
+
+        // release our Mats from native memory
+        objectPoints.release();
+        imagePoints.release();
+        cameraMatrixMat.release();
+        distCoeffsMat.release();
+        for (var v : rvecs) v.release();
+        for (var v : tvecs) v.release();
+        rvec.release();
+        tvec.release();
+        reprojectionError.release();
+
+        if (alt != null) return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]);
+        else return new PNPResults(best, errors[0]);
+    }
+    /**
+     * Finds the transformation that maps the camera's pose to the target pose. The camera's pose
+     * relative to the target is determined by the supplied 3d points of the target's model and their
+     * associated 2d points imaged by the camera.
+     *
+     * <p>This method is intended for use with multiple targets and has no alternate solutions. There
+     * must be at least 3 points.
+     *
+     * @param cameraMatrix the camera intrinsics matrix in standard opencv form
+     * @param distCoeffs the camera distortion matrix in standard opencv form
+     * @param objectTrls The translations of the object corners, relative to the field.
+     * @param imageCorners The projection of these 3d object points into the 2d camera image. The
+     *     order should match the given object point translations.
+     * @return The resulting transformation that maps the camera pose to the target pose. If the 3d
+     *     model points are supplied relative to the origin, this transformation brings the camera to
+     *     the origin.
+     */
+    public static PNPResults solvePNP_SQPNP(
+            Matrix<N3, N3> cameraMatrix,
+            Matrix<N5, N1> distCoeffs,
+            List<Translation3d> objectTrls,
+            List<TargetCorner> imageCorners) {
+        // translate to opencv classes
+        var objectPoints = translationToTvec(objectTrls.toArray(new Translation3d[0]));
+        var imagePoints = targetCornersToMat(imageCorners);
+        var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
+        var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
+        var rvecs = new ArrayList<Mat>();
+        var tvecs = new ArrayList<Mat>();
+        var rvec = Mat.zeros(3, 1, CvType.CV_32F);
+        var tvec = Mat.zeros(3, 1, CvType.CV_32F);
+        var reprojectionError = new Mat();
+        // calc rvec/tvec from image points
+        Calib3d.solvePnPGeneric(
+                objectPoints,
+                imagePoints,
+                cameraMatrixMat,
+                distCoeffsMat,
+                rvecs,
+                tvecs,
+                false,
+                Calib3d.SOLVEPNP_SQPNP,
+                rvec,
+                tvec,
+                reprojectionError);
+
+        float[] error = new float[1];
+        reprojectionError.get(0, 0, error);
+        // convert to wpilib coordinates
+        var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
+
+        // release our Mats from native memory
+        objectPoints.release();
+        imagePoints.release();
+        cameraMatrixMat.release();
+        distCoeffsMat.release();
+        for (var v : rvecs) v.release();
+        for (var v : tvecs) v.release();
+        rvec.release();
+        tvec.release();
+        reprojectionError.release();
+
+        return new PNPResults(best, error[0]);
+    }
+}

photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java

@@ -0,0 +1,72 @@
+/*
+ * 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.estimation;
+
+import edu.wpi.first.math.geometry.Transform3d;
+
+/**
+ * The best estimated transformation from solvePnP, and possibly an alternate transformation
+ * depending on the solvePNP method. If an alternate solution is present, the ambiguity value
+ * represents the ratio of reprojection error in the best solution to the alternate (best /
+ * alternate).
+ *
+ * <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
+ * method.
+ */
+public class PNPResults {
+    public final Transform3d best;
+    public final double bestReprojErr;
+
+    /**
+     * Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
+     * to the best solution.
+     */
+    public final Transform3d alt;
+    /** If no alternate solution is found, this is bestReprojErr */
+    public final double altReprojErr;
+    /** If no alternate solution is found, this is 0 */
+    public final double ambiguity;
+
+    public PNPResults() {
+        this(new Transform3d(), new Transform3d(), 0, 0, 0);
+    }
+
+    public PNPResults(Transform3d best, double bestReprojErr) {
+        this(best, best, 0, bestReprojErr, bestReprojErr);
+    }
+
+    public PNPResults(
+            Transform3d best,
+            Transform3d alt,
+            double ambiguity,
+            double bestReprojErr,
+            double altReprojErr) {
+        this.best = best;
+        this.alt = alt;
+        this.ambiguity = ambiguity;
+        this.bestReprojErr = bestReprojErr;
+        this.altReprojErr = altReprojErr;
+    }
+}

photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java

@@ -0,0 +1,114 @@
+/*
+ * 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.estimation;
+
+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 java.util.List;
+import java.util.stream.Collectors;
+
+/**
+ * Represents a transformation that first rotates a pose around the origin, and then translates it.
+ */
+public class RotTrlTransform3d {
+    private final Translation3d trl;
+    private final Rotation3d rot;
+
+    public RotTrlTransform3d() {
+        this(new Rotation3d(), new Translation3d());
+    }
+    /**
+     * Creates a rotation-translation transformation from a Transform3d.
+     *
+     * <p>Applying this transformation to poses will preserve their current origin-to-pose transform
+     * as if the origin was transformed by these components.
+     *
+     * @param trf The origin transformation
+     */
+    public RotTrlTransform3d(Transform3d trf) {
+        this(trf.getRotation(), trf.getTranslation());
+    }
+    /**
+     * A rotation-translation transformation.
+     *
+     * <p>Applying this transformation to poses will preserve their current origin-to-pose transform
+     * as if the origin was transformed by these components.
+     *
+     * @param rot The rotation component
+     * @param trl The translation component
+     */
+    public RotTrlTransform3d(Rotation3d rot, Translation3d trl) {
+        this.rot = rot;
+        this.trl = trl;
+    }
+    /**
+     * The rotation-translation transformation that makes poses in the world consider this pose as the
+     * new origin, or change the basis to this pose.
+     *
+     * @param pose The new origin
+     */
+    public static RotTrlTransform3d makeRelativeTo(Pose3d pose) {
+        return new RotTrlTransform3d(pose.getRotation(), pose.getTranslation()).inverse();
+    }
+
+    /** The inverse of this transformation. Applying the inverse will "undo" this transformation. */
+    public RotTrlTransform3d inverse() {
+        var inverseRot = rot.unaryMinus();
+        var inverseTrl = trl.rotateBy(inverseRot).unaryMinus();
+        return new RotTrlTransform3d(inverseRot, inverseTrl);
+    }
+
+    /** This transformation as a Transform3d (as if of the origin) */
+    public Transform3d getTransform() {
+        return new Transform3d(trl, rot);
+    }
+    /** The translation component of this transformation */
+    public Translation3d getTranslation() {
+        return trl;
+    }
+    /** The rotation component of this transformation */
+    public Rotation3d getRotation() {
+        return rot;
+    }
+
+    public Translation3d apply(Translation3d trl) {
+        return apply(new Pose3d(trl, new Rotation3d())).getTranslation();
+    }
+    ;
+
+    public List<Translation3d> applyTrls(List<Translation3d> trls) {
+        return trls.stream().map(t -> apply(t)).collect(Collectors.toList());
+    }
+
+    public Pose3d apply(Pose3d pose) {
+        return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl), pose.getRotation().plus(rot));
+    }
+
+    public List<Pose3d> applyPoses(List<Pose3d> poses) {
+        return poses.stream().map(p -> apply(p)).collect(Collectors.toList());
+    }
+}

photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java

@@ -0,0 +1,116 @@
+/*
+ * 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.estimation;
+
+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.math.util.Units;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.stream.Collectors;
+
+/** Describes the 3d model of a target. */
+public class TargetModel {
+    /**
+     * Translations of this target's vertices relative to its pose. If this target is spherical, this
+     * list has one translation with x == radius.
+     */
+    public final List<Translation3d> vertices;
+
+    public final boolean isPlanar;
+    public final boolean isSpherical;
+
+    public static final TargetModel kTag16h5 =
+            new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6));
+
+    /** Creates a rectangular, planar target model given the width and height. */
+    public TargetModel(double widthMeters, double heightMeters) {
+        this.vertices =
+                List.of(
+                        // this order is relevant for AprilTag compatibility
+                        new Translation3d(0, -widthMeters / 2.0, -heightMeters / 2.0),
+                        new Translation3d(0, widthMeters / 2.0, -heightMeters / 2.0),
+                        new Translation3d(0, widthMeters / 2.0, heightMeters / 2.0),
+                        new Translation3d(0, -widthMeters / 2.0, heightMeters / 2.0));
+        this.isPlanar = true;
+        this.isSpherical = false;
+    }
+
+    /**
+     * Creates a spherical target model which has similar dimensions when viewed from any angle. This
+     * model will only have one vertex which has x == radius.
+     */
+    public TargetModel(double diameterMeters) {
+        this.vertices = List.of(new Translation3d(diameterMeters / 2.0, 0, 0));
+        this.isPlanar = false;
+        this.isSpherical = true;
+    }
+
+    /**
+     * Creates a target model from arbitrary 3d vertices. Automatically determines if the given
+     * vertices are planar(x == 0). More than 2 vertices must be given.
+     *
+     * @param vertices Translations representing the vertices of this target model relative to its
+     *     pose.
+     */
+    public TargetModel(List<Translation3d> vertices) {
+        this.isSpherical = false;
+        if (vertices == null || vertices.size() <= 2) {
+            vertices = new ArrayList<>();
+            this.isPlanar = false;
+        } else {
+            boolean cornersPlanar = true;
+            for (Translation3d corner : vertices) {
+                if (corner.getX() != 0) cornersPlanar = false;
+            }
+            this.isPlanar = cornersPlanar;
+        }
+        this.vertices = vertices;
+    }
+
+    /**
+     * This target's vertices offset from its field pose.
+     *
+     * <p>Note: If this target is spherical, only one vertex radius meters in front of the pose is
+     * returned.
+     */
+    public List<Translation3d> getFieldVertices(Pose3d targetPose) {
+        return vertices.stream()
+                .map(t -> targetPose.plus(new Transform3d(t, new Rotation3d())).getTranslation())
+                .collect(Collectors.toList());
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj instanceof TargetModel) {
+            var o = (TargetModel) obj;
+            return vertices.equals(o.vertices) && isPlanar == o.isPlanar && isSpherical == o.isSpherical;
+        }
+        return false;
+    }
+}

photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java

@@ -0,0 +1,168 @@
+/*
+ * 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.estimation;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.*;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.ArrayList;
+import java.util.List;
+import org.photonvision.targeting.TargetCorner;
+
+public class VisionEstimation {
+    public static final TargetModel kTagModel =
+            new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6));
+
+    /**
+     * Performs solvePNP using 3d-2d point correspondences to estimate the field-to-camera
+     * transformation. If only one tag is visible, the result may have an alternate solution.
+     *
+     * <p><b>Note:</b> The returned transformation is from the field origin to the camera pose!
+     *
+     * @param cameraMatrix the camera intrinsics matrix in standard opencv form
+     * @param distCoeffs the camera distortion matrix in standard opencv form
+     * @param corners The visible tag corners in the 2d image
+     * @param knownTags The known tag field poses corresponding to the visible tag IDs
+     * @return The transformation that maps the field origin to the camera pose
+     */
+    public static PNPResults estimateCamPosePNP(
+            Matrix<N3, N3> cameraMatrix,
+            Matrix<N5, N1> distCoeffs,
+            List<TargetCorner> corners,
+            List<AprilTag> knownTags) {
+        if (knownTags == null
+                || corners == null
+                || corners.size() != knownTags.size() * 4
+                || knownTags.size() == 0) {
+            return new PNPResults();
+        }
+        // single-tag pnp
+        if (corners.size() == 4) {
+            var camToTag =
+                    OpenCVHelp.solvePNP_SQUARE(
+                            cameraMatrix, distCoeffs, kTagModel.getFieldVertices(knownTags.get(0).pose), corners);
+            var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
+            var altPose = new Pose3d();
+            if (camToTag.ambiguity != 0)
+                altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse());
+
+            var bestTagToCam = camToTag.best.inverse();
+            SmartDashboard.putNumberArray(
+                    "multiTagBest_internal",
+                    new double[] {
+                        bestTagToCam.getX(),
+                        bestTagToCam.getY(),
+                        bestTagToCam.getZ(),
+                        bestTagToCam.getRotation().getQuaternion().getW(),
+                        bestTagToCam.getRotation().getQuaternion().getX(),
+                        bestTagToCam.getRotation().getQuaternion().getY(),
+                        bestTagToCam.getRotation().getQuaternion().getZ()
+                    });
+
+            var o = new Pose3d();
+            return new PNPResults(
+                    new Transform3d(o, bestPose),
+                    new Transform3d(o, altPose),
+                    camToTag.ambiguity,
+                    camToTag.bestReprojErr,
+                    camToTag.altReprojErr);
+        }
+        // multi-tag pnp
+        else {
+            var objectTrls = new ArrayList<Translation3d>();
+            for (var tag : knownTags) objectTrls.addAll(kTagModel.getFieldVertices(tag.pose));
+            var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners);
+            // var camToOrigin = OpenCVHelp.solveTagsPNPRansac(prop, objectTrls, corners);
+            return new PNPResults(
+                    camToOrigin.best.inverse(),
+                    camToOrigin.alt.inverse(),
+                    camToOrigin.ambiguity,
+                    camToOrigin.bestReprojErr,
+                    camToOrigin.altReprojErr);
+        }
+    }
+
+    // /**
+    //  * The best estimated transformation to the target, and possibly an alternate
+    //  * transformation
+    //  * depending on the solvePNP method. If an alternate solution is present, the
+    //  * ambiguity value
+    //  * represents the ratio of reprojection error in the best solution to the
+    //  * alternate (best / alternate).
+    //  */
+    // public static class PNPResults {
+    //     public final Transform3d best;
+    //     public final double bestReprojErr;
+
+    //     /**
+    //      * Alternate, ambiguous solution from solvepnp. This may be empty
+    //      * if no alternate solution is found.
+    //      */
+    //     public final Transform3d alt;
+    //     /** If no alternate solution is found, this is 0 */
+    //     public final double altReprojErr;
+
+    //     /** If no alternate solution is found, this is 0 */
+    //     public final double ambiguity;
+
+    //     public PNPResults() {
+    //         this(new Transform3d(), new Transform3d(), 0, 0, 0);
+    //     }
+
+    //     public PNPResults(
+    //             Transform3d best, Transform3d alt,
+    //             double ambiguity, double bestReprojErr, double altReprojErr) {
+    //         this.best = best;
+    //         this.alt = alt;
+    //         this.ambiguity = ambiguity;
+    //         this.bestReprojErr = bestReprojErr;
+    //         this.altReprojErr = altReprojErr;
+    //     }
+    // }
+
+    /**
+     * The best estimated transformation (Rotation-translation composition) that maps a set of
+     * translations onto another with point correspondences, and its RMSE.
+     */
+    public static class SVDResults {
+        public final RotTrlTransform3d trf;
+        /** If the result is invalid, this value is -1 */
+        public final double rmse;
+
+        public SVDResults() {
+            this(new RotTrlTransform3d(), -1);
+        }
+
+        public SVDResults(RotTrlTransform3d trf, double rmse) {
+            this.trf = trf;
+            this.rmse = rmse;
+        }
+    }
+}

photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp

@@ -26,6 +26,7 @@
 
 #include <frc/Errors.h>
 #include <frc/Timer.h>
+#include <opencv2/core/mat.hpp>
 
 #include "PhotonVersion.h"
 #include "photonlib/Packet.h"
@@ -41,8 +42,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(
@@ -51,14 +51,21 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
           rootTable->GetIntegerTopic("outputSaveImgCmd").Publish()),
       outputSaveImgSubscriber(
           rootTable->GetIntegerTopic("outputSaveImgCmd").Subscribe(0)),
-      pipelineIndexEntry(rootTable->GetIntegerTopic("pipelineIndex").Publish()),
-      ledModeEntry(mainTable->GetIntegerTopic("ledMode").Publish()),
+      pipelineIndexPub(
+          rootTable->GetIntegerTopic("pipelineIndexRequest").Publish()),
+      pipelineIndexSub(
+          rootTable->GetIntegerTopic("pipelineIndexState").Subscribe(0)),
+      ledModePub(mainTable->GetIntegerTopic("ledMode").Publish()),
+      ledModeSub(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
       versionEntry(mainTable->GetStringTopic("version").Subscribe("")),
+      cameraIntrinsicsSubscriber(
+          rootTable->GetDoubleArrayTopic("cameraIntrinsics").Subscribe({})),
+      cameraDistortionSubscriber(
+          rootTable->GetDoubleArrayTopic("cameraDistortion").Subscribe({})),
       driverModeSubscriber(
           rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
-      pipelineIndexSubscriber(
-          rootTable->GetIntegerTopic("pipelineIndex").Subscribe(-1)),
-      ledModeSubscriber(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
+      driverModePublisher(
+          rootTable->GetBooleanTopic("driverModeRequest").Publish()),
       m_topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}),
       path(rootTable->GetPath()),
       m_cameraName(cameraName) {}
@@ -67,7 +74,10 @@ PhotonCamera::PhotonCamera(const std::string_view cameraName)
     : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
 
 PhotonPipelineResult PhotonCamera::GetLatestResult() {
-  if (test) return testResult;
+  if (test) {
+    return testResult;
+  }
+
   // Prints warning if not connected
   VerifyVersion();
 
@@ -92,7 +102,7 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
 }
 
 void PhotonCamera::SetDriverMode(bool driverMode) {
-  driverModeEntry.Set(driverMode);
+  driverModePublisher.Set(driverMode);
 }
 
 void PhotonCamera::TakeInputSnapshot() {
@@ -106,23 +116,37 @@ void PhotonCamera::TakeOutputSnapshot() {
 bool PhotonCamera::GetDriverMode() const { return driverModeSubscriber.Get(); }
 
 void PhotonCamera::SetPipelineIndex(int index) {
-  pipelineIndexEntry.Set(static_cast<double>(index));
+  pipelineIndexPub.Set(static_cast<double>(index));
 }
 
 int PhotonCamera::GetPipelineIndex() const {
-  return static_cast<int>(pipelineIndexSubscriber.Get());
+  return static_cast<int>(pipelineIndexSub.Get());
 }
 
 LEDMode PhotonCamera::GetLEDMode() const {
-  return static_cast<LEDMode>(static_cast<int>(ledModeSubscriber.Get()));
+  return static_cast<LEDMode>(static_cast<int>(ledModeSub.Get()));
+}
+
+std::optional<cv::Mat> PhotonCamera::GetCameraMatrix() {
+  auto camCoeffs = cameraIntrinsicsSubscriber.Get();
+  if (camCoeffs.size() == 9) {
+    // clone should deal with ownership concerns? not sure
+    return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone();
+  }
+  return std::nullopt;
 }
 
 void PhotonCamera::SetLEDMode(LEDMode mode) {
-  ledModeEntry.Set(static_cast<double>(static_cast<int>(mode)));
+  ledModePub.Set(static_cast<double>(static_cast<int>(mode)));
 }
 
-const std::string_view PhotonCamera::GetCameraName() const {
-  return m_cameraName;
+std::optional<cv::Mat> PhotonCamera::GetDistCoeffs() {
+  auto distCoeffs = cameraDistortionSubscriber.Get();
+  if (distCoeffs.size() == 5) {
+    // clone should deal with ownership concerns? not sure
+    return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone();
+  }
+  return std::nullopt;
 }
 
 void PhotonCamera::VerifyVersion() {

photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp

@@ -0,0 +1,428 @@
+/*
+ * 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 <cmath>
+#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 <opencv2/calib3d.hpp>
+#include <opencv2/core/mat.hpp>
+#include <opencv2/core/types.hpp>
+#include <units/math.h>
+#include <units/time.h>
+
+#include "photonlib/PhotonCamera.h"
+#include "photonlib/PhotonPipelineResult.h"
+#include "photonlib/PhotonTrackedTarget.h"
+
+namespace photonlib {
+
+namespace detail {
+cv::Point3d ToPoint3d(const frc::Translation3d& translation);
+std::optional<std::array<cv::Point3d, 4>> CalcTagCorners(
+    int tagID, const frc::AprilTagFieldLayout& aprilTags);
+frc::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec);
+cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
+                                   units::meter_t cornerY, frc::Pose3d tagPose);
+}  // namespace detail
+
+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()),
+      poseCacheTimestamp(-1_s) {}
+
+void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
+  if (strategy == MULTI_TAG_PNP) {
+    FRC_ReportError(
+        frc::warn::Warning,
+        "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
+        "");
+    strategy = LOWEST_AMBIGUITY;
+  }
+  if (this->multiTagFallbackStrategy != strategy) {
+    InvalidatePoseCache();
+  }
+  multiTagFallbackStrategy = strategy;
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
+  auto result = camera.GetLatestResult();
+  return Update(result);
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
+    const PhotonPipelineResult& result) {
+  // Time in the past -- give up, since the following if expects times > 0
+  if (result.GetTimestamp() < 0_s) {
+    return std::nullopt;
+  }
+
+  // If the pose cache timestamp was set, and the result is from the same
+  // timestamp, return an empty result
+  if (poseCacheTimestamp > 0_s &&
+      units::math::abs(poseCacheTimestamp - result.GetTimestamp()) < 0.001_ms) {
+    return std::nullopt;
+  }
+
+  // Remember the timestamp of the current result used
+  poseCacheTimestamp = result.GetTimestamp();
+
+  // If no targets seen, trivial case -- return empty result
+  if (!result.HasTargets()) {
+    return std::nullopt;
+  }
+
+  return Update(result, this->strategy);
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
+    PhotonPipelineResult result, PoseStrategy strategy) {
+  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;
+    case ::photonlib::MULTI_TAG_PNP:
+      ret = MultiTagPnpStrategy(result);
+      break;
+    default:
+      FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
+                      "");
+      ret = std::nullopt;
+  }
+
+  return ret;
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
+    PhotonPipelineResult result) {
+  double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
+  auto targets = result.GetTargets();
+  auto foundIt = targets.end();
+  for (auto it = targets.begin(); it != targets.end(); ++it) {
+    if (it->GetPoseAmbiguity() < lowestAmbiguityScore) {
+      foundIt = it;
+      lowestAmbiguityScore = it->GetPoseAmbiguity();
+    }
+  }
+
+  if (foundIt == targets.end()) {
+    return std::nullopt;
+  }
+
+  auto& bestTarget = *foundIt;
+
+  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(), result.GetTargets()};
+}
+
+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 const targetPose = fiducialPose.value();
+
+    units::meter_t const alternativeDifference = units::math::abs(
+        m_robotToCamera.Z() -
+        targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
+            .Z());
+
+    units::meter_t const 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(), result.GetTargets()};
+    }
+    if (bestDifference < smallestHeightDifference) {
+      smallestHeightDifference = bestDifference;
+      pose = EstimatedRobotPose{
+          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
+              .TransformBy(m_robotToCamera.Inverse()),
+          result.GetTimestamp(), result.GetTargets()};
+    }
+  }
+
+  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 (auto& target : targets) {
+    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 const alternativeDifference = units::math::abs(
+        referencePose.Translation().Distance(altPose.Translation()));
+    units::meter_t const 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, result.GetTargets()};
+}
+
+std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
+    int tagID, const frc::AprilTagFieldLayout& aprilTags) {
+  if (auto tagPose = aprilTags.GetTagPose(tagID); tagPose.has_value()) {
+    return std::array{TagCornerToObjectPoint(-3_in, -3_in, *tagPose),
+                      TagCornerToObjectPoint(+3_in, -3_in, *tagPose),
+                      TagCornerToObjectPoint(+3_in, +3_in, *tagPose),
+                      TagCornerToObjectPoint(-3_in, +3_in, *tagPose)};
+  } else {
+    return std::nullopt;
+  }
+}
+
+cv::Point3d detail::ToPoint3d(const frc::Translation3d& translation) {
+  return cv::Point3d(-translation.Y().value(), -translation.Z().value(),
+                     +translation.X().value());
+}
+
+cv::Point3d detail::TagCornerToObjectPoint(units::meter_t cornerX,
+                                           units::meter_t cornerY,
+                                           frc::Pose3d tagPose) {
+  frc::Translation3d cornerTrans =
+      tagPose.Translation() +
+      frc::Translation3d(0.0_m, cornerX, cornerY).RotateBy(tagPose.Rotation());
+  return ToPoint3d(cornerTrans);
+}
+
+frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
+  using namespace frc;
+  using namespace units;
+
+  cv::Mat R;
+  cv::Rodrigues(rvec, R);  // R is 3x3
+
+  R = R.t();                  // rotation of inverse
+  cv::Mat tvecI = -R * tvec;  // translation of inverse
+
+  Vectord<3> tv;
+  tv[0] = +tvecI.at<double>(2, 0);
+  tv[1] = -tvecI.at<double>(0, 0);
+  tv[2] = -tvecI.at<double>(1, 0);
+  Vectord<3> rv;
+  rv[0] = +rvec.at<double>(2, 0);
+  rv[1] = -rvec.at<double>(0, 0);
+  rv[2] = +rvec.at<double>(1, 0);
+
+  return Pose3d(Translation3d(meter_t{tv[0]}, meter_t{tv[1]}, meter_t{tv[2]}),
+                Rotation3d(
+                    // radian_t{rv[0]},
+                    // radian_t{rv[1]},
+                    // radian_t{rv[2]}
+                    rv, radian_t{rv.norm()}));
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
+    PhotonPipelineResult result) {
+  using namespace frc;
+
+  if (!result.HasTargets() || result.GetTargets().size() < 2) {
+    return Update(result, this->multiTagFallbackStrategy);
+  }
+
+  auto const targets = result.GetTargets();
+
+  // List of corners mapped from 3d space (meters) to the 2d camera screen
+  // (pixels).
+  std::vector<cv::Point3f> objectPoints;
+  std::vector<cv::Point2f> imagePoints;
+
+  // Add all target corners to main list of corners
+  for (auto target : targets) {
+    int id = target.GetFiducialId();
+    if (auto const tagCorners = detail::CalcTagCorners(id, aprilTags);
+        tagCorners.has_value()) {
+      auto const targetCorners = target.GetDetectedCorners();
+      for (size_t cornerIdx = 0; cornerIdx < 4; ++cornerIdx) {
+        imagePoints.emplace_back(targetCorners[cornerIdx].first,
+                                 targetCorners[cornerIdx].second);
+        objectPoints.emplace_back((*tagCorners)[cornerIdx]);
+      }
+    }
+  }
+
+  if (imagePoints.empty()) {
+    return std::nullopt;
+  }
+
+  // Use OpenCV ITERATIVE solver
+  cv::Mat const rvec(3, 1, cv::DataType<double>::type);
+  cv::Mat const tvec(3, 1, cv::DataType<double>::type);
+
+  auto const camMat = camera.GetCameraMatrix();
+  auto const distCoeffs = camera.GetDistCoeffs();
+  if (!camMat || !distCoeffs) {
+    return std::nullopt;
+  }
+
+  cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(),
+               rvec, tvec, false, cv::SOLVEPNP_SQPNP);
+
+  Pose3d const pose = detail::ToPose3d(tvec, rvec);
+
+  return photonlib::EstimatedRobotPose(
+      pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
+      result.GetTargets());
+}
+
+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 (auto& target : targets) {
+    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.GetTimestamp(), result.GetTargets()};
+    }
+    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 const 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(), result.GetTargets()};
+}
+}  // namespace photonlib

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 &&

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

photon-lib/src/main/native/include/photonlib/PhotonCamera.h

@@ -28,6 +28,7 @@
 #include <string>
 
 #include <networktables/BooleanTopic.h>
+#include <networktables/DoubleArrayTopic.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/IntegerTopic.h>
 #include <networktables/MultiSubscriber.h>
@@ -40,6 +41,10 @@
 
 #include "photonlib/PhotonPipelineResult.h"
 
+namespace cv {
+class Mat;
+}  // namespace cv
+
 namespace photonlib {
 
 enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 };
@@ -68,6 +73,8 @@ class PhotonCamera {
    */
   explicit PhotonCamera(const std::string_view cameraName);
 
+  PhotonCamera(PhotonCamera&&) = default;
+
   virtual ~PhotonCamera() = default;
 
   /**
@@ -142,6 +149,9 @@ class PhotonCamera {
    */
   const std::string_view GetCameraName() const;
 
+  std::optional<cv::Mat> GetCameraMatrix();
+  std::optional<cv::Mat> GetDistCoeffs();
+
   /**
    * Returns whether the latest target result has targets.
    * This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should
@@ -166,17 +176,21 @@ 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;
   nt::IntegerSubscriber outputSaveImgSubscriber;
-  nt::IntegerPublisher pipelineIndexEntry;
-  nt::IntegerPublisher ledModeEntry;
+  nt::IntegerPublisher pipelineIndexPub;
+  nt::IntegerSubscriber pipelineIndexSub;
+  nt::IntegerPublisher ledModePub;
+  nt::IntegerSubscriber ledModeSub;
   nt::StringSubscriber versionEntry;
 
+  nt::DoubleArraySubscriber cameraIntrinsicsSubscriber;
+  nt::DoubleArraySubscriber cameraDistortionSubscriber;
+
   nt::BooleanSubscriber driverModeSubscriber;
-  nt::IntegerSubscriber pipelineIndexSubscriber;
+  nt::BooleanPublisher driverModePublisher;
   nt::IntegerSubscriber ledModeSubscriber;
 
   nt::MultiSubscriber m_topicNameSubscriber;

photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h

@@ -0,0 +1,257 @@
+/*
+ * 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 <memory>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Transform3d.h>
+
+#include "photonlib/PhotonCamera.h"
+#include "photonlib/PhotonPipelineResult.h"
+
+namespace cv {
+class Mat;
+}  // namespace cv
+
+namespace photonlib {
+enum PoseStrategy {
+  LOWEST_AMBIGUITY = 0,
+  CLOSEST_TO_CAMERA_HEIGHT,
+  CLOSEST_TO_REFERENCE_POSE,
+  CLOSEST_TO_LAST_POSE,
+  AVERAGE_BEST_TARGETS,
+  MULTI_TAG_PNP
+};
+
+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;
+
+  /** A list of the targets used to compute this pose */
+  wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
+
+  EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
+                     std::span<const PhotonTrackedTarget> targets)
+      : estimatedPose(pose_),
+        timestamp(time_),
+        targetsUsed(targets.data(), targets.data() + targets.size()) {}
+};
+
+/**
+ * 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:
+  /**
+   * 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) {
+    if (strategy != strat) {
+      InvalidatePoseCache();
+    }
+    strategy = strat;
+  }
+
+  /**
+   * Set the Position Estimation Strategy used in multi-tag mode when
+   * only one tag can be seen. Must NOT be MULTI_TAG_PNP
+   *
+   * @param strategy the strategy to set
+   */
+  void SetMultiTagFallbackStrategy(PoseStrategy strategy);
+
+  /**
+   * 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) {
+    if (this->referencePose != referencePose) {
+      InvalidatePoseCache();
+    }
+    this->referencePose = referencePose;
+  }
+
+  /**
+   * @return The current transform from the center of the robot to the camera
+   *         mount position.
+   */
+  inline frc::Transform3d GetRobotToCameraTransform() {
+    return m_robotToCamera;
+  }
+
+  /**
+   * Useful for pan and tilt mechanisms, or cameras on turrets
+   *
+   * @param robotToCamera The current transform from the center of the robot to
+   * the camera mount position.
+   */
+  inline void SetRobotToCameraTransform(frc::Transform3d robotToCamera) {
+    m_robotToCamera = robotToCamera;
+  }
+
+  /**
+   * 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();
+
+  /**
+   * Update the pose estimator.
+   */
+  std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result);
+
+  inline PhotonCamera& GetCamera() { return camera; }
+
+ private:
+  frc::AprilTagFieldLayout aprilTags;
+  PoseStrategy strategy;
+  PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
+
+  PhotonCamera camera;
+  frc::Transform3d m_robotToCamera;
+
+  frc::Pose3d lastPose;
+  frc::Pose3d referencePose;
+
+  units::second_t poseCacheTimestamp;
+
+  inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
+
+  std::optional<EstimatedRobotPose> Update(PhotonPipelineResult result,
+                                           PoseStrategy strategy);
+
+  /**
+   * 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 pose calculation using all targets in view in the same PNP()
+   calculation
+
+   * @return the estimated position of the robot in the FCS and the estimated
+   timestamp of this estimation.
+   */
+  std::optional<EstimatedRobotPose> MultiTagPnpStrategy(
+      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

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).

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

photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h

@@ -47,7 +47,7 @@ class SimPhotonCamera : public PhotonCamera {
     targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
     targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
     targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
-    rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("raw");
+    rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("rawBytes");
     versionEntry = instance.GetTable("photonvision")->GetEntry("version");
     // versionEntry.SetString(PhotonVersion.versionString);
   }

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}}});
       }
 

photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java

@@ -25,11 +25,12 @@
 package org.photonvision;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.hal.JNIWrapper;
-import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
@@ -45,12 +46,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 +63,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 +121,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 +141,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);
@@ -165,8 +158,6 @@ class RobotPoseEstimatorTest {
 
     @Test
     void testClosestToCameraHeightStrategy() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
-
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
         cameraOne.result =
                 new PhotonPipelineResult(
@@ -209,12 +200,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 +221,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 +241,6 @@ class RobotPoseEstimatorTest {
 
     @Test
     void closestToReferencePoseStrategy() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
-
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
         cameraOne.result =
                 new PhotonPipelineResult(
@@ -296,12 +283,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 +303,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 +324,6 @@ class RobotPoseEstimatorTest {
 
     @Test
     void closestToLastPose() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
-
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
         cameraOne.result =
                 new PhotonPipelineResult(
@@ -384,12 +366,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,
@@ -409,17 +386,19 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
+        cameraOne.result.setTimestampSeconds(1);
 
-        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,20 +461,84 @@ 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);
     }
 
     @Test
-    void averageBestPoses() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
+    void cacheIsInvalidated() {
+        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
+        var result =
+                new PhotonPipelineResult(
+                        2,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        0,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+        result.setTimestampSeconds(20);
 
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.AVERAGE_BEST_TARGETS,
+                        cameraOne,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
+
+        // Empty result, expect empty result
+        cameraOne.result = new PhotonPipelineResult();
+        cameraOne.result.setTimestampSeconds(1);
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        assertFalse(estimatedPose.isPresent());
+
+        // Set actual result
+        cameraOne.result = result;
+        estimatedPose = estimator.update();
+        assertTrue(estimatedPose.isPresent());
+        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+
+        // And again -- pose cache should mean this is empty
+        cameraOne.result = result;
+        estimatedPose = estimator.update();
+        assertFalse(estimatedPose.isPresent());
+        // Expect the old timestamp to still be here
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+
+        // Set new field layout -- right after, the pose cache timestamp should be -1
+        estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
+        assertEquals(-1, estimator.poseCacheTimestampSeconds);
+        // Update should cache the current timestamp (20) again
+        cameraOne.result = result;
+        estimatedPose = estimator.update();
+        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+    }
+
+    @Test
+    void averageBestPoses() {
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
         cameraOne.result =
                 new PhotonPipelineResult(
@@ -542,12 +581,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 +601,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);

photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java

@@ -0,0 +1,101 @@
+/*
+ * 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.estimation;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.cscore.CameraServerCvJNI;
+import edu.wpi.first.hal.JNIWrapper;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.net.WPINetJNI;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.util.CombinedRuntimeLoader;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.io.IOException;
+import org.junit.jupiter.api.BeforeAll;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+
+public class ApriltagWorkbenchTest {
+    @BeforeAll
+    public static void setUp() {
+        JNIWrapper.Helper.setExtractOnStaticLoad(false);
+        WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
+        NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
+        WPINetJNI.Helper.setExtractOnStaticLoad(false);
+        CameraServerCvJNI.Helper.setExtractOnStaticLoad(false);
+
+        try {
+            CombinedRuntimeLoader.loadLibraries(
+                    ApriltagWorkbenchTest.class,
+                    "wpiutiljni",
+                    "ntcorejni",
+                    "wpinetjni",
+                    "wpiHaljni",
+                    "cscorejnicvstatic");
+        } catch (Exception e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+        }
+
+        // No version check for testing
+        PhotonCamera.setVersionCheckEnabled(false);
+    }
+
+    // @Test
+    public void testMeme() throws IOException, InterruptedException {
+        NetworkTableInstance instance = NetworkTableInstance.getDefault();
+        instance.stopServer();
+        // set the NT server if simulating this code.
+        // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]"
+        // for coprocessor
+        instance.setServer("localhost");
+        instance.startClient4("myRobot");
+
+        var robotToCamera = new Transform3d();
+        var cam = new PhotonCamera("WPI2023");
+        var tagLayout =
+                AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile);
+
+        var pe =
+                new PhotonPoseEstimator(
+                        tagLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, cam, robotToCamera);
+
+        var field = new Field2d();
+        SmartDashboard.putData(field);
+
+        while (!Thread.interrupted()) {
+            Thread.sleep(500);
+
+            var ret = pe.update();
+            System.out.println(ret);
+
+            field.setRobotPose(ret.get().estimatedPose.toPose2d());
+        }
+    }
+}

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}}}};
 

photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp

@@ -0,0 +1,363 @@
+/*
+ * 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();
+  ASSERT_TRUE(estimatedPose);
+  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(21));
+
+  estimatedPose = estimator.Update();
+  ASSERT_TRUE(estimatedPose);
+  pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(21.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);
+}
+
+TEST(PhotonPoseEstimatorTest, PoseCache) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test2");
+
+  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;
+
+  photonlib::PhotonPoseEstimator estimator(
+      aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
+
+  // empty input, expect empty out
+  estimator.GetCamera().testResult = {2_ms, {}};
+  estimator.GetCamera().testResult.SetTimestamp(units::second_t(1));
+  auto estimatedPose = estimator.Update();
+  EXPECT_FALSE(estimatedPose);
+
+  // Set result, and update -- expect present and timestamp to be 15
+  estimator.GetCamera().testResult = {3_ms, targets};
+  estimator.GetCamera().testResult.SetTimestamp(units::second_t(15));
+  estimatedPose = estimator.Update();
+  EXPECT_TRUE(estimatedPose);
+  EXPECT_NEAR(15, estimatedPose.value().timestamp.to<double>(), 1e-6);
+
+  // And again -- now pose cache should be empty
+  estimatedPose = estimator.Update();
+  EXPECT_FALSE(estimatedPose);
+}

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);

photon-server/networktables.json

@@ -0,0 +1 @@
+[]

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);

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);
     }

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);
     }

photon-targeting/build.gradle

@@ -5,6 +5,11 @@ apply from: "${rootDir}/shared/common.gradle"
 dependencies {
     implementation wpilibTools.deps.wpilibJava("wpimath")
     implementation "org.ejml:ejml-simple:0.41"
+
+    // Junit
+    testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
+    testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
+    testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2")
 }
 
 tasks.withType(JavaCompile) {
@@ -19,4 +24,8 @@ java {
     withSourcesJar()
 }
 
+test {
+    useJUnitPlatform()
+}
+
 apply from: "publish.gradle"

photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java

@@ -182,4 +182,18 @@ public class Packet {
         }
         return packetData[readPos++] == 1;
     }
+
+    public void encode(double[] data) {
+        for (double d : data) {
+            encode(d);
+        }
+    }
+
+    public double[] decodeDoubleArray(int len) {
+        double[] ret = new double[len];
+        for (int i = 0; i < len; i++) {
+            ret[i] = decodeDouble();
+        }
+        return ret;
+    }
 }

photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java

@@ -41,9 +41,8 @@ public class NTTopicSet {
     public NetworkTable subTable;
     public RawPublisher rawBytesEntry;
 
-    public IntegerTopic pipelineIndexTopic;
     public IntegerPublisher pipelineIndexPublisher;
-    public IntegerSubscriber pipelineIndexSubscriber;
+    public IntegerSubscriber pipelineIndexRequestSub;
 
     public BooleanTopic driverModeEntry;
     public BooleanPublisher driverModePublisher;
@@ -65,19 +64,24 @@ public class NTTopicSet {
     public IntegerTopic heartbeatTopic;
     public IntegerPublisher heartbeatPublisher;
 
+    // Camera Calibration
+    public DoubleArrayPublisher cameraIntrinsicsPublisher;
+    public DoubleArrayPublisher cameraDistortionPublisher;
+
     public void updateEntries() {
         rawBytesEntry =
                 subTable
                         .getRawTopic("rawBytes")
                         .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
 
-        pipelineIndexTopic = subTable.getIntegerTopic("pipelineIndex");
-        pipelineIndexPublisher = pipelineIndexTopic.publish();
-        pipelineIndexSubscriber = pipelineIndexTopic.subscribe(0);
+        pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish();
+        pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").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();
@@ -93,13 +97,16 @@ public class NTTopicSet {
 
         heartbeatTopic = subTable.getIntegerTopic("heartbeat");
         heartbeatPublisher = heartbeatTopic.publish();
+
+        cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish();
+        cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish();
     }
 
     @SuppressWarnings("DuplicatedCode")
     public void removeEntries() {
         if (rawBytesEntry != null) rawBytesEntry.close();
         if (pipelineIndexPublisher != null) pipelineIndexPublisher.close();
-        if (pipelineIndexSubscriber != null) pipelineIndexSubscriber.close();
+        if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close();
 
         if (driverModePublisher != null) driverModePublisher.close();
         if (driverModeSubscriber != null) driverModeSubscriber.close();
@@ -113,5 +120,10 @@ public class NTTopicSet {
         if (targetSkewEntry != null) targetSkewEntry.close();
         if (bestTargetPosX != null) bestTargetPosX.close();
         if (bestTargetPosY != null) bestTargetPosY.close();
+
+        if (heartbeatPublisher != null) heartbeatPublisher.close();
+
+        if (cameraIntrinsicsPublisher != null) cameraIntrinsicsPublisher.close();
+        if (cameraDistortionPublisher != null) cameraDistortionPublisher.close();
     }
 }

photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java

@@ -19,7 +19,6 @@ package org.photonvision.targeting;
 
 import java.util.ArrayList;
 import java.util.List;
-import java.util.Objects;
 import org.photonvision.common.dataflow.structures.Packet;
 
 /** Represents a pipeline result from a PhotonCamera. */
@@ -123,21 +122,6 @@ public class PhotonPipelineResult {
         return new ArrayList<>(targets);
     }
 
-    @Override
-    public boolean equals(Object o) {
-        if (this == o) return true;
-        if (o == null || getClass() != o.getClass()) return false;
-        PhotonPipelineResult that = (PhotonPipelineResult) o;
-        boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0;
-        boolean targetsMatch = that.targets.equals(targets);
-        return latencyMatch && targetsMatch;
-    }
-
-    @Override
-    public int hashCode() {
-        return Objects.hash(latencyMillis, targets);
-    }
-
     /**
      * Populates the fields of the pipeline result from the packet.
      *
@@ -178,4 +162,33 @@ public class PhotonPipelineResult {
         // Return the packet.
         return packet;
     }
+
+    @Override
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        result = prime * result + ((targets == null) ? 0 : targets.hashCode());
+        long temp;
+        temp = Double.doubleToLongBits(latencyMillis);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(timestampSeconds);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj == null) return false;
+        if (getClass() != obj.getClass()) return false;
+        PhotonPipelineResult other = (PhotonPipelineResult) obj;
+        if (targets == null) {
+            if (other.targets != null) return false;
+        } else if (!targets.equals(other.targets)) return false;
+        if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis))
+            return false;
+        if (Double.doubleToLongBits(timestampSeconds)
+                != Double.doubleToLongBits(other.timestampSeconds)) return false;
+        return true;
+    }
 }

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.3.2"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

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.3.2"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

photonlib-cpp-examples/apriltagExample/.gitignore

@@ -0,0 +1 @@
+vendordeps

photonlib-cpp-examples/apriltagExample/.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": true,
+    "currentLanguage": "cpp",
+    "projectYear": "2023",
+    "teamNumber": 5
+}

photonlib-cpp-examples/apriltagExample/WPILib-License.md

@@ -0,0 +1,24 @@
+Copyright (c) 2009-2021 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

photonlib-cpp-examples/apriltagExample/build.gradle

@@ -0,0 +1,116 @@
+plugins {
+    id "cpp"
+    id "google-test-test-suite"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
+
+    id "com.dorongold.task-tree" version "2.1.0"
+}
+
+repositories {
+    mavenLocal()
+    jcenter()
+}
+
+apply from: "${rootDir}/../shared/examples_common.gradle"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+    targets {
+        roborio(getTargetTypeClass('RoboRIO')) {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = project.frc.getTeamOrDefault(5940)
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcCpp(getArtifactTypeClass('FRCNativeArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Set to true to run simulation in debug mode
+wpi.cpp.debugSimulation = false
+
+// Default enable simgui
+wpi.sim.addGui().defaultEnabled = true
+// Enable DS but not by default
+wpi.sim.addDriverstation()
+
+model {
+    components {
+        frcUserProgram(NativeExecutableSpec) {
+            // We don't need to build for roborio -- if we do, we need to install
+            // a roborio toolchain every time we build in CI
+            // Ideally, we'd be able to set the roborio toolchain as optional, but
+            // I can't figure out how to set that environment variable from build.gradle
+            // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71)
+            // for now, commented out
+
+            // targetPlatform wpi.platforms.roborio
+
+            if (includeDesktopSupport) {
+                targetPlatform wpi.platforms.desktop
+            }
+
+            sources.cpp {
+                source {
+                    srcDir 'src/main/cpp'
+                    include '**/*.cpp', '**/*.cc'
+                }
+                exportedHeaders {
+                    srcDir 'src/main/include'
+                }
+            }
+
+            // Set deploy task to deploy this component
+            deployArtifact.component = it
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            // Enable simulation for this component
+            wpi.sim.enable(it)
+            // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+        }
+    }
+    testSuites {
+        frcUserProgramTest(GoogleTestTestSuiteSpec) {
+            testing $.components.frcUserProgram
+
+            sources.cpp {
+                source {
+                    srcDir 'src/test/cpp'
+                    include '**/*.cpp'
+                }
+            }
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+            wpi.cpp.deps.googleTest(it)
+        }
+    }
+}

photonlib-cpp-examples/apriltagExample/gradlew

@@ -0,0 +1,240 @@
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 the original authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+##############################################################################
+#
+#   Gradle start up script for POSIX generated by Gradle.
+#
+#   Important for running:
+#
+#   (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+#       noncompliant, but you have some other compliant shell such as ksh or
+#       bash, then to run this script, type that shell name before the whole
+#       command line, like:
+#
+#           ksh Gradle
+#
+#       Busybox and similar reduced shells will NOT work, because this script
+#       requires all of these POSIX shell features:
+#         * functions;
+#         * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+#           «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+#         * compound commands having a testable exit status, especially «case»;
+#         * various built-in commands including «command», «set», and «ulimit».
+#
+#   Important for patching:
+#
+#   (2) This script targets any POSIX shell, so it avoids extensions provided
+#       by Bash, Ksh, etc; in particular arrays are avoided.
+#
+#       The "traditional" practice of packing multiple parameters into a
+#       space-separated string is a well documented source of bugs and security
+#       problems, so this is (mostly) avoided, by progressively accumulating
+#       options in "$@", and eventually passing that to Java.
+#
+#       Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+#       and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+#       see the in-line comments for details.
+#
+#       There are tweaks for specific operating systems such as AIX, CygWin,
+#       Darwin, MinGW, and NonStop.
+#
+#   (3) This script is generated from the Groovy template
+#       https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+    APP_HOME=${app_path%"${app_path##*/}"}  # leaves a trailing /; empty if no leading path
+    [ -h "$app_path" ]
+do
+    ls=$( ls -ld "$app_path" )
+    link=${ls#*' -> '}
+    case $link in             #(
+      /*)   app_path=$link ;; #(
+      *)    app_path=$APP_HOME$link ;;
+    esac
+done
+
+APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
+
+APP_NAME="Gradle"
+APP_BASE_NAME=${0##*/}
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+    echo "$*"
+} >&2
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in                #(
+  CYGWIN* )         cygwin=true  ;; #(
+  Darwin* )         darwin=true  ;; #(
+  MSYS* | MINGW* )  msys=true    ;; #(
+  NONSTOP* )        nonstop=true ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD=$JAVA_HOME/jre/sh/java
+    else
+        JAVACMD=$JAVA_HOME/bin/java
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD=java
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+#   * args from the command line
+#   * the main class name
+#   * -classpath
+#   * -D...appname settings
+#   * --module-path (only if needed)
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if "$cygwin" || "$msys" ; then
+    APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+    JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    for arg do
+        if
+            case $arg in                                #(
+              -*)   false ;;                            # don't mess with options #(
+              /?*)  t=${arg#/} t=/${t%%/*}              # looks like a POSIX filepath
+                    [ -e "$t" ] ;;                      #(
+              *)    false ;;
+            esac
+        then
+            arg=$( cygpath --path --ignore --mixed "$arg" )
+        fi
+        # Roll the args list around exactly as many times as the number of
+        # args, so each arg winds up back in the position where it started, but
+        # possibly modified.
+        #
+        # NB: a `for` loop captures its iteration list before it begins, so
+        # changing the positional parameters here affects neither the number of
+        # iterations, nor the values presented in `arg`.
+        shift                   # remove old arg
+        set -- "$@" "$arg"      # push replacement arg
+    done
+fi
+
+# Collect all arguments for the java command;
+#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+#     shell script including quotes and variable substitutions, so put them in
+#     double quotes to make sure that they get re-expanded; and
+#   * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
+
+exec "$JAVACMD" "$@"

photonlib-cpp-examples/apriltagExample/gradlew.bat

@@ -0,0 +1,91 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega

photonlib-cpp-examples/apriltagExample/networktables.json

@@ -0,0 +1 @@
+[]

photonlib-cpp-examples/apriltagExample/settings.gradle

@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+rootProject.name = 'aimattarget'
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        jcenter()
+        gradlePluginPortal()
+        String frcYear = '2023'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name 'frcHome'
+            url frcHomeMaven
+        }
+    }
+}

photonlib-cpp-examples/apriltagExample/simgui-ds.json

@@ -0,0 +1,102 @@
+{
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {},
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decayRate": 0.0,
+          "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 65,
+          "incKey": 68
+        }
+      ],
+      "axisCount": 6,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

photonlib-cpp-examples/apriltagExample/simgui-window.json

@@ -0,0 +1,81 @@
+{
+  "Docking": {
+    "Data": []
+  },
+  "MainWindow": {
+    "GLOBAL": {
+      "fps": "120",
+      "height": "880",
+      "maximized": "0",
+      "style": "0",
+      "userScale": "2",
+      "width": "1652",
+      "xpos": "268",
+      "ypos": "82"
+    }
+  },
+  "Table": {
+    "0x542B5671,2": {
+      "Column 0  Width": "391",
+      "Column 1  Width": "156",
+      "RefScale": "13"
+    }
+  },
+  "Window": {
+    "###/SmartDashboard/Field": {
+      "Collapsed": "0",
+      "Pos": "514,2",
+      "Size": "517,341"
+    },
+    "###FMS": {
+      "Collapsed": "0",
+      "Pos": "36,663",
+      "Size": "283,146"
+    },
+    "###Joysticks": {
+      "Collapsed": "0",
+      "Pos": "373,500",
+      "Size": "796,206"
+    },
+    "###Keyboard 0 Settings": {
+      "Collapsed": "0",
+      "Pos": "149,98",
+      "Size": "300,560"
+    },
+    "###NetworkTables": {
+      "Collapsed": "0",
+      "Pos": "663,464",
+      "Size": "750,365"
+    },
+    "###NetworkTables Info": {
+      "Collapsed": "0",
+      "Pos": "520,330",
+      "Size": "750,145"
+    },
+    "###Other Devices": {
+      "Collapsed": "0",
+      "Pos": "1025,20",
+      "Size": "250,695"
+    },
+    "###System Joysticks": {
+      "Collapsed": "0",
+      "Pos": "5,350",
+      "Size": "192,218"
+    },
+    "###Timing": {
+      "Collapsed": "0",
+      "Pos": "5,150",
+      "Size": "135,127"
+    },
+    "Debug##Default": {
+      "Collapsed": "0",
+      "Pos": "60,60",
+      "Size": "400,400"
+    },
+    "Robot State": {
+      "Collapsed": "0",
+      "Pos": "5,20",
+      "Size": "92,99"
+    }
+  }
+}

photonlib-cpp-examples/apriltagExample/simgui.json

@@ -0,0 +1,40 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Field": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "CameraPublisher": {
+        "open": true
+      },
+      "SmartDashboard": {
+        "open": true
+      },
+      "photonvision": {
+        "open": true,
+        "photonvision": {
+          "open": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "Server": {
+      "open": true
+    },
+    "visible": true
+  }
+}

photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp

@@ -0,0 +1,92 @@
+/*
+ * 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 "Drivetrain.h"
+
+#include <frc/RobotController.h>
+#include <frc/smartdashboard/Field2d.h>
+
+void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
+  auto leftFeedforward = m_feedforward.Calculate(speeds.left);
+  auto rightFeedforward = m_feedforward.Calculate(speeds.right);
+  double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(),
+                                                    speeds.left.value());
+  double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(),
+                                                      speeds.right.value());
+
+  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::radians_per_second_t rot) {
+  SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_poseEstimator.Update(m_gyro.GetRotation2d(),
+                         units::meter_t{m_leftEncoder.GetDistance()},
+                         units::meter_t{m_rightEncoder.GetDistance()});
+}
+
+void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+  m_drivetrainSimulator.SetPose(pose);
+  m_poseEstimator.ResetPosition(
+      m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
+      units::meter_t{m_rightEncoder.GetDistance()}, pose);
+}
+
+void Drivetrain::SimulationPeriodic() {
+  // To update our simulation, we set motor voltage inputs, update the
+  // simulation, and write the simulated positions and velocities to our
+  // simulated encoder and gyro. We negate the right side so that positive
+  // voltages make the right side move forward.
+
+  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftGroup.Get()} *
+                                      frc::RobotController::GetInputVoltage(),
+                                  units::volt_t{m_rightGroup.Get()} *
+                                      frc::RobotController::GetInputVoltage());
+  m_drivetrainSimulator.Update(20_ms);
+
+  m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
+  m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
+  m_rightEncoderSim.SetDistance(
+      m_drivetrainSimulator.GetRightPosition().value());
+  m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
+  m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value());
+}
+
+void Drivetrain::Periodic() {
+  UpdateOdometry();
+
+  auto result = m_pcw.Update(m_poseEstimator.GetEstimatedPosition());
+  if (result) {
+    m_poseEstimator.AddVisionMeasurement(
+        result.value().estimatedPose.ToPose2d(), result.value().timestamp);
+  }
+
+  m_fieldSim.SetRobotPose(m_poseEstimator.GetEstimatedPosition());
+}

photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp

@@ -0,0 +1,61 @@
+/*
+ * 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 "Robot.h"
+
+#include <frc/RobotBase.h>
+#include <networktables/NetworkTable.h>
+
+void Robot::RobotInit() {
+  if constexpr (frc::RobotBase::IsSimulation()) {
+    auto inst = nt::NetworkTableInstance::GetDefault();
+    inst.StopServer();
+    // set the NT server if simulating this code.
+    // "localhost" for photon on desktop, or "photonvision.local" or
+    // "[ip-address]" for coprocessor
+    inst.SetServer("localhost");
+    inst.StartClient4("Robot Simulation");
+  }
+}
+
+void Robot::TeleopPeriodic() {
+  // Get the x speed. We are inverting this because Xbox controllers return
+  // negative values when we push forward.
+  const auto xSpeed = -m_controller.GetLeftY() * Drivetrain::kMaxSpeed;
+
+  // Get the rate of angular rotation. We are inverting this because we want a
+  // positive value when we pull to the left (remember, CCW is positive in
+  // mathematics). Xbox controllers return positive values when you pull to
+  // the right by default.
+  auto rot = -m_controller.GetRightX() * Drivetrain::kMaxAngularSpeed;
+
+  m_drive.Drive(xSpeed, rot);
+}
+
+void Robot::RobotPeriodic() { m_drive.Periodic(); }
+void Robot::SimulationPeriodic() { m_drive.SimulationPeriodic(); }
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h

@@ -0,0 +1,140 @@
+/*
+ * 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 <numbers>
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/estimator/DifferentialDrivePoseEstimator.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/AnalogGyroSim.h>
+#include <frc/simulation/DifferentialDrivetrainSim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/smartdashboard/Field2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+
+#include "PhotonCameraWrapper.h"
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() {
+    m_gyro.Reset();
+
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.SetInverted(true);
+
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
+                                      kEncoderResolution);
+    m_rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
+                                       kEncoderResolution);
+
+    m_leftEncoder.Reset();
+    m_rightEncoder.Reset();
+
+    m_rightGroup.SetInverted(true);
+
+    frc::SmartDashboard::PutData("Field", &m_fieldSim);
+  }
+
+  static constexpr units::meters_per_second_t kMaxSpeed =
+      3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      std::numbers::pi};  // 1/2 rotation per second
+
+  void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
+  void Drive(units::meters_per_second_t xSpeed,
+             units::radians_per_second_t rot);
+  void UpdateOdometry();
+  void ResetOdometry(const frc::Pose2d& pose);
+
+  frc::Pose2d GetPose() const { return m_poseEstimator.GetEstimatedPosition(); }
+
+  void SimulationPeriodic();
+  void Periodic();
+
+ private:
+  static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
+  static constexpr double kWheelRadius = 0.0508;  // meters
+  static constexpr int kEncoderResolution = 4096;
+
+  frc::PWMSparkMax m_leftLeader{1};
+  frc::PWMSparkMax m_leftFollower{2};
+  frc::PWMSparkMax m_rightLeader{3};
+  frc::PWMSparkMax m_rightFollower{4};
+
+  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
+  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
+
+  frc::Encoder m_leftEncoder{0, 1};
+  frc::Encoder m_rightEncoder{2, 3};
+
+  frc2::PIDController m_leftPIDController{8.5, 0.0, 0.0};
+  frc2::PIDController m_rightPIDController{8.5, 0.0, 0.0};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
+
+  frc::DifferentialDrivePoseEstimator m_poseEstimator{
+      m_kinematics, m_gyro.GetRotation2d(),
+      units::meter_t{m_leftEncoder.GetDistance()},
+      units::meter_t{m_rightEncoder.GetDistance()}, frc::Pose2d{}};
+
+  PhotonCameraWrapper m_pcw;
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
+
+  // Simulation classes help us simulate our robot
+  frc::sim::AnalogGyroSim m_gyroSim{m_gyro};
+  frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
+  frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
+  frc::Field2d m_fieldSim;
+  frc::LinearSystem<2, 2, 2> m_drivetrainSystem =
+      frc::LinearSystemId::IdentifyDrivetrainSystem(
+          1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
+  frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
+      m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in};
+};

photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h

@@ -0,0 +1,47 @@
+/*
+ * 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 <photonlib/PhotonCamera.h>
+#include <photonlib/PhotonPoseEstimator.h>
+
+#include <utility>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+
+class PhotonCameraWrapper {
+ public:
+  photonlib::PhotonPoseEstimator m_poseEstimator{
+      frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
+      photonlib::MULTI_TAG_PNP, std::move(photonlib::PhotonCamera{"WPI2023"}),
+      frc::Transform3d{}};
+
+  inline std::optional<photonlib::EstimatedRobotPose> Update(
+      frc::Pose2d estimatedPose) {
+    m_poseEstimator.SetReferencePose(frc::Pose3d(estimatedPose));
+    return m_poseEstimator.Update();
+  }
+};

photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h

@@ -24,20 +24,27 @@
 
 #pragma once
 
-#include <Eigen/Core>
+#include <photonlib/PhotonCamera.h>
 
-namespace frc {
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMVictorSPX.h>
+#include <units/angle.h>
+#include <units/length.h>
 
-template <int Size>
-using Vectord = Eigen::Vector<double, Size>;
+#include "Drivetrain.h"
 
-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>;
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void TeleopPeriodic() override;
+  void SimulationPeriodic() override;
 
-}  // namespace frc
+ private:
+  frc::XboxController m_controller{0};
+
+  Drivetrain m_drive;
+};

photonlib-cpp-examples/apriltagExample/src/test/cpp/main.cpp

@@ -0,0 +1,34 @@
+/*
+ * 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 <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

photonlib-cpp-examples/examples.txt

@@ -1,3 +1,4 @@
 aimandrange
 getinrange
 aimattarget
+apriltagExample

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.3.2"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

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.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

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.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

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.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

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 {
@@ -180,16 +181,14 @@ public class Drivetrain {
         m_poseEstimator.update(
                 m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
 
-        // 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()));

photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java

@@ -24,80 +24,53 @@
 
 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.apriltag.AprilTagFields;
 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 edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.Constants.VisionConstants;
-import java.util.ArrayList;
+import java.io.IOException;
 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;
+    private PhotonCamera photonCamera;
+    private PhotonPoseEstimator photonPoseEstimator;
 
     public PhotonCameraWrapper() {
-        // Set up a test arena of two apriltags at the center of each driver station set
-        final AprilTag tag18 =
-                new AprilTag(
-                        18,
-                        new Pose3d(
-                                new Pose2d(
-                                        FieldConstants.length,
-                                        FieldConstants.width / 2.0,
-                                        Rotation2d.fromDegrees(180))));
-        final AprilTag tag01 =
-                new AprilTag(
-                        01,
-                        new Pose3d(new Pose2d(0.0, FieldConstants.width / 2.0, Rotation2d.fromDegrees(0.0))));
-        ArrayList<AprilTag> atList = new ArrayList<AprilTag>();
-        atList.add(tag18);
-        atList.add(tag01);
+        // Change the name of your camera here to whatever it is in the PhotonVision UI.
+        photonCamera = new PhotonCamera(VisionConstants.cameraName);
 
-        // TODO - once 2023 happens, replace this with just loading the 2023 field arrangement
-        AprilTagFieldLayout atfl =
-                new AprilTagFieldLayout(atList, FieldConstants.length, FieldConstants.width);
-
-        // Forward Camera
-        photonCamera =
-                new PhotonCamera(
-                        VisionConstants
-                                .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);
+        try {
+            // Attempt to load the AprilTagFieldLayout that will tell us where the tags are on the field.
+            AprilTagFieldLayout fieldLayout = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField();
+            // Create pose estimator
+            photonPoseEstimator =
+                    new PhotonPoseEstimator(
+                            fieldLayout, PoseStrategy.MULTI_TAG_PNP, photonCamera, VisionConstants.robotToCam);
+            photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+        } catch (IOException e) {
+            // The AprilTagFieldLayout failed to load. We won't be able to estimate poses if we don't know
+            // where the tags are.
+            DriverStation.reportError("Failed to load AprilTagFieldLayout", e.getStackTrace());
+            photonPoseEstimator = null;
+        }
     }
 
     /**
      * @param estimatedRobotPose The current best guess at robot pose
-     * @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
+     * @return an EstimatedRobotPose with an estimated pose, the timestamp, and targets used to create
+     *     the estimate
      */
-    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) {
+        if (photonPoseEstimator == null) {
+            // The field layout failed to load, so we cannot estimate poses.
+            return Optional.empty();
         }
+        photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
+        return photonPoseEstimator.update();
     }
 }

photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java

@@ -46,7 +46,7 @@ public class Robot extends TimedRobot {
             // set the NT server if simulating this code.
             // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]" for coprocessor
             instance.setServer("localhost");
-            instance.startClient4("myRobot");
+            instance.startClient4("Robot Simulation");
         }
 
         m_controller = new XboxController(0);

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.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

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.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

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.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

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

shared/config.gradle

@@ -8,9 +8,9 @@ nativeUtils.withCrossLinuxArm64()
 nativeUtils.wpi.configureDependencies {
     wpiVersion = wpilibVersion
     wpimathVersion = wpilibVersion
-    niLibVersion = "2023.2.0"
-    opencvVersion = "4.6.0-3"
-    googleTestVersion = "1.11.0-4"
+    niLibVersion = "2023.3.0"
+    opencvVersion = "4.6.0-4"
+    googleTestVersion = "1.12.1-1"
     imguiVersion = "1.86-1"
 }
 

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
+}