Bump wpilib to 2023.4.1 (#811)

* 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).
@@ -156,10 +157,11 @@ jobs:
           pip install sphinx sphinx_rtd_theme sphinx-tabs sphinxext-opengraph doc8
           pip install -r requirements.txt
 
-      - name: Check the docs
-        run: |
-          make linkcheck
-          make lint
+      # Don't check the docs. If a PR was merged to the docs repo, it ought to pass CI. No need to re-check here.
+      # - name: Check the docs
+      #   run: |
+      #     make linkcheck
+      #     make lint
 
       - name: Build the docs
         run: |
@@ -375,30 +377,57 @@ jobs:
           ./gradlew photon-server:shadowJar --max-workers 2
         if: ${{ (matrix.arch-override == 'none') }}
 
-      # The image will only pull the Pi32 JAR in
-      - name: Generate image
-        if: ${{ github.event_name != 'pull_request' && (matrix.artifact-name) == 'LinuxArm32' }}
-        run: |
-          chmod +x scripts/generatePiImage.sh
-          ./scripts/generatePiImage.sh
-
       # Upload final fat jar as artifact.
       - uses: actions/upload-artifact@v3
         with:
           name: jar-${{ matrix.artifact-name }}
           path: photon-server/build/libs
 
-      # Upload image as well
+
+  photon-image-generator:
+    needs: [photon-build-package]
+    if: ${{ github.event_name != 'pull_request' }}
+    strategy:
+        fail-fast: false
+        matrix:
+          include:
+            - os: ubuntu-latest
+              artifact-name: LinuxArm64
+              image_suffix: RaspberryPi
+              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: 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 }}"
+
+    steps:
+      # Checkout code.
+      - name: Checkout code
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+
+      - uses: actions/download-artifact@v2
+        with:
+          name: jar-${{ matrix.artifact-name }}
+
+      - name: Generate image
+        run: |
+          chmod +x scripts/generatePiImage.sh
+          ./scripts/generatePiImage.sh ${{ matrix.image_url }} ${{ matrix.image_suffix }}
 
       - uses: actions/upload-artifact@v3
-        if: ${{ github.event_name != 'pull_request' && (matrix.artifact-name) == 'LinuxArm32' }}
+        name: Upload image
         with:
-          name: image-${{ matrix.artifact-name }}
+          name: image-${{ matrix.image_suffix }}
           path: photonvision*.xz
 
 
   photon-release:
-    needs: [photon-build-package]
+    needs: [photon-build-package, photon-image-generator]
     runs-on: ubuntu-22.04
     steps:
       # Download literally every single artifact. This also downloads client and docs,

.gitignore

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

build.gradle

@@ -4,7 +4,7 @@ plugins {
     id "com.github.node-gradle.node" version "3.1.1" apply false
     id "edu.wpi.first.GradleJni" version "1.0.0"
     id "edu.wpi.first.GradleVsCode" version "1.1.0"
-    id "edu.wpi.first.NativeUtils" version "2023.10.0" apply false
+    id "edu.wpi.first.NativeUtils" version "2023.11.1" apply false
     id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2"
     id "org.hidetake.ssh" version "2.10.1"
     id 'edu.wpi.first.WpilibTools' version '1.0.0'
@@ -26,24 +26,20 @@ allprojects {
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2023.1.1-beta-7-15-g1e7fcd5"
+    wpilibVersion = "2023.4.1"
     opencvVersion = "4.6.0-4"
     joglVersion = "2.4.0-rc-20200307"
     pubVersion = versionString
     isDev = pubVersion.startsWith("dev")
 
-    if(project.hasProperty('pionly')) {
-        jniPlatforms = ['linuxarm32']
-    } else if(project.hasProperty('winonly')) {
-        jniPlatforms = ['windowsx86-64']
-    } else if(project.hasProperty('aarch64only')) {
-        jniPlatforms = ['linuxaarch64bionic']
-    } else {
-        jniPlatforms = ['linuxarm64', 'linuxarm32', 'linuxx86-64', 'osxuniversal', 'windowsx86-64']
-    }
-
-    println("Building for archs " + jniPlatforms)
-
+    // A list, for legacy reasons, with only the current platform contained
+    String nativeName = wpilibTools.platformMapper.currentPlatform.platformName;
+    if (nativeName == "linuxx64") nativeName = "linuxx86-64";
+    if (nativeName == "winx64") nativeName = "windowsx86-64";
+    if (nativeName == "macx64") nativeName = "osxx86-64";
+    if (nativeName == "macarm64") nativeName = "osxarm64";
+    jniPlatform = nativeName
+    println("Building for platform " + jniPlatform)
 }
 
 wpilibTools.deps.wpilibVersion = wpilibVersion

photon-client/public/thinclient.html

@@ -304,7 +304,7 @@
             document.getElementById("host").value = host_in;
         }
 
-        if(port_in != "" & host_in != ""){
+        if(port_in != "" && host_in != ""){
             streamStartRequest(); //we got valid inputs, auto-start the stream
         }
 

photon-client/src/App.vue

@@ -134,7 +134,7 @@
 
 <script>
 import Logs from "./views/LogsView"
-// import {mapState} from "vuex";
+import { ReconnectingWebsocket } from "./plugins/ReconnectingWebsocket.js"
 
 export default {
   name: 'App',
@@ -145,7 +145,8 @@ export default {
     // Used so that we can switch back to the previously selected pipeline after camera calibration
     previouslySelectedIndices: [],
     timer: undefined,
-    teamNumberDialog: true
+    teamNumberDialog: true,
+    websocket: null,
   }),
   computed: {
     needsTeamNumberSet: {
@@ -190,15 +191,12 @@ export default {
       }
     });
 
-    this.recreateWebsocket();
-  },
-  methods: {
-    recreateWebsocket() {
-      const wsDataURL = 'ws://' + this.$address + '/websocket_data';
-      let socket = new WebSocket(wsDataURL);
-      socket.binaryType = "arraybuffer";
+    const wsDataURL = 'ws://' + this.$address + '/websocket_data';
+    this.websocket = new ReconnectingWebsocket(
+      wsDataURL,
 
-      socket.onmessage = (event) => {
+      // On data in
+      (event) => {
         try {
           let message = this.$msgPack.decode(event.data);
           for (let prop in message) {
@@ -210,31 +208,21 @@ export default {
           console.log(event)
           console.error('error: ' + JSON.stringify(event.data) + " , " + error);
         }
-      };
+      },
 
-      socket.onerror = () => {
-        socket.close();
-        this.$store.commit("backendConnected", false)
-      };
+      // on connect
+      (event) => {
+        event; this.$store.commit("backendConnected", true);
+        this.$store.state.connectedCallbacks.forEach(it => it());
+       },
 
+      // on disconnect
+      (event) => { event; this.$store.commit("backendConnected", false) }
+    );
 
-      socket.onopen = () => {
-        clearInterval(this.timerId);
-
-        socket.onclose = () => {
-          this.$store.commit("backendConnected", false)
-          this.timerId = setInterval(() => {
-            this.recreateWebsocket();
-          }, 1000);
-        };
-
-
-        this.$store.commit("backendConnected", true)
-        this.$store.state.connectedCallbacks.forEach(it => it())
-      };
-
-      this.$store.commit("websocket", socket);
-    },
+    this.$store.commit("websocket", this.websocket);
+  },
+  methods: {
     handleMessage(key, value) {
       if (key === "logMessage") {
         this.logMessage(value["logMessage"], value["logLevel"]);

photon-client/src/components/common/cv-image.vue

@@ -4,7 +4,7 @@
     crossOrigin="anonymous"
     :style="styleObject"
     :src="src"
-    alt=""
+    :alt="alt"
     @click="clickHandler"
     @error="loadErrHandler"
   />
@@ -14,7 +14,7 @@
     export default {
         name: "CvImage",
         // eslint-disable-next-line vue/require-prop-types
-        props: ['address', 'scale', 'maxHeight', 'maxHeightMd', 'maxHeightLg', 'maxHeightXl', 'colorPicking', 'id', 'disconnected'],
+        props: ['address', 'scale', 'maxHeight', 'maxHeightMd', 'maxHeightLg', 'maxHeightXl', 'colorPicking', 'id', 'disconnected', 'alt'],
         data() {
             return {
                 seed: 1.0,

photon-client/src/components/common/cv-select.vue

@@ -37,7 +37,7 @@ import TooltippedLabel from "./cv-tooltipped-label";
             TooltippedLabel,
         },
       // eslint-disable-next-line vue/require-prop-types
-        props: ['list', 'name', 'value', 'disabled', 'selectCols', 'rules', 'tooltip'],
+        props: ['list', 'name', 'value', 'disabled', 'filteredIndices', 'selectCols', 'rules', 'tooltip'],
         computed: {
             localValue: {
                 get() {
@@ -50,6 +50,7 @@ import TooltippedLabel from "./cv-tooltipped-label";
             indexList() {
                 let list = [];
                 for (let i = 0; i < this.list.length; i++) {
+                    if (this.filteredIndices instanceof Set && this.filteredIndices.has(i)) continue;
                     list.push({
                         name: this.list[i],
                         index: i

photon-client/src/components/pipeline/CameraAndPipelineSelect.vue

@@ -257,7 +257,7 @@ export default {
   },
   data: () => {
     return {
-      re: RegExp("^[A-Za-z0-9 \\-)(]*[A-Za-z0-9][A-Za-z0-9 \\-)(.]*$"),
+      re: RegExp("^[A-Za-z0-9_ \\-)(]*[A-Za-z0-9][A-Za-z0-9_ \\-)(.]*$"),
       isCameraNameEdit: false,
       newCameraName: "",
       cameraNameError: "",

photon-client/src/mixins/global/dataHandleMixin.js

@@ -2,14 +2,14 @@ export const dataHandleMixin = {
     methods: {
         handleInput(key, value) {
             let msg = this.$msgPack.encode({[key]: value});
-            this.$store.state.websocket.send(msg);
+            this.$store.state.websocket.ws.send(msg);
         },
         handleInputWithIndex(key, value, cameraIndex = this.$store.getters.currentCameraIndex) {
             let msg = this.$msgPack.encode({
                 [key]: value,
                 ["cameraIndex"]: cameraIndex,
             });
-            this.$store.state.websocket.send(msg);
+            this.$store.state.websocket.ws.send(msg);
         },
         handleData(val) {
             this.handleInput(val, this[val]);
@@ -22,7 +22,7 @@ export const dataHandleMixin = {
                     ["cameraIndex"]: this.$store.getters.currentCameraIndex
                 }
             });
-            this.$store.state.websocket.send(msg);
+            this.$store.state.websocket.ws.send(msg);
             this.$emit('update')
         },
         handlePipelineUpdate(key, val) {
@@ -32,7 +32,7 @@ export const dataHandleMixin = {
                     ["cameraIndex"]: this.$store.getters.currentCameraIndex
                 }
             });
-            this.$store.state.websocket.send(msg);
+            this.$store.state.websocket.ws.send(msg);
             this.$emit('update')
         },
         handleTruthyPipelineData(val) {
@@ -42,7 +42,7 @@ export const dataHandleMixin = {
                     ["cameraIndex"]: this.$store.getters.currentCameraIndex
                 }
             });
-            this.$store.state.websocket.send(msg);
+            this.$store.state.websocket.ws.send(msg);
             this.$emit('update')
         },
         rollback(val, e) {

photon-client/src/plugins/ReconnectingWebsocket.js

@@ -0,0 +1,74 @@
+/**
+ * Auto-reconnecting Websocket, a stripped down version of the NT4 client from
+ * https://raw.githubusercontent.com/wpilibsuite/NetworkTablesClients/2f8d378ac08d5ca703d590cfb019fc4af062db89/nt4/js/src/nt4.js
+ */
+export class ReconnectingWebsocket {
+    constructor(serverAddr,
+        onDataIn_in,
+        onConnect_in,
+        onDisconnect_in) {
+
+        this.onDataIn = onDataIn_in;
+        this.onConnect = onConnect_in;
+        this.onDisconnect = onDisconnect_in;
+
+        // WS Connection State (with defaults)
+        this.serverAddr = serverAddr;
+        this.serverConnectionActive = false;
+
+        //Trigger the websocket to connect automatically
+        this.ws_connect();
+    }
+
+    //////////////////////////////////////////////////////////////
+    // Websocket connection Maintenance
+
+    ws_onOpen() {
+        // Set the flag allowing general server communication
+        this.serverConnectionActive = true;
+
+        console.log("[WebSocket] Connected!");
+
+        // User connection-opened hook
+        this.onConnect();
+    }
+
+    ws_onClose(e) {
+        //Clear flags to stop server communication
+        this.ws = null;
+        this.serverConnectionActive = false;
+
+        // User connection-closed hook
+        this.onDisconnect();
+
+        console.log('[WebSocket] Socket is closed. Reconnect will be attempted in 0.5 second.', e.reason);
+        setTimeout(this.ws_connect.bind(this), 500);
+
+        if (!e.wasClean) {
+            console.error('Socket encountered error!');
+        }
+
+    }
+
+    ws_onError(e) {
+        console.log("[WebSocket] Websocket error - " + e.toString());
+        this.ws.close();
+    }
+
+    ws_onMessage(e) {
+       this.onDataIn(e);
+    }
+
+    ws_connect() {
+        this.ws = new WebSocket(this.serverAddr);
+        this.ws.binaryType = "arraybuffer";
+        this.ws.onopen = this.ws_onOpen.bind(this);
+        this.ws.onmessage = this.ws_onMessage.bind(this);
+        this.ws.onclose = this.ws_onClose.bind(this);
+        this.ws.onerror = this.ws_onError.bind(this);
+
+        console.log("[WebSocket] Starting...");
+    }
+}
+
+export default { ReconnectingWebsocket }

photon-client/src/plugins/objectDiff.js

@@ -1,121 +0,0 @@
-//https://gomakethings.com/getting-the-differences-between-two-objects-with-vanilla-js/
-export const diff = function (obj1, obj2) {
-
-    // Make sure an object to compare is provided
-    if (!obj2 || Object.prototype.toString.call(obj2) !== '[object Object]') {
-        return obj1;
-    }
-
-    //
-    // Variables
-    //
-
-    let diffs = {};
-    let key;
-
-
-    //
-    // Methods
-    //
-
-    /**
-     * Check if two arrays are equal
-     * @param  {Array}   arr1 The first array
-     * @param  {Array}   arr2 The second array
-     * @return {Boolean}      If true, both arrays are equal
-     */
-    const arraysMatch = function (arr1, arr2) {
-
-        // Check if the arrays are the same length
-        if (arr1.length !== arr2.length) return false;
-
-        // Check if all items exist and are in the same order
-        for (let i = 0; i < arr1.length; i++) {
-            if (arr1[i] !== arr2[i]) return false;
-        }
-
-        // Otherwise, return true
-        return true;
-
-    };
-
-    /**
-     * Compare two items and push non-matches to object
-     * @param  {*}      item1 The first item
-     * @param  {*}      item2 The second item
-     * @param  {String} key   The key in our object
-     */
-    const compare = function (item1, item2, key) {
-
-        // Get the object type
-        let type1 = Object.prototype.toString.call(item1);
-        let type2 = Object.prototype.toString.call(item2);
-
-        // If type2 is undefined it has been removed
-        if (type2 === '[object Undefined]') {
-            diffs[key] = null;
-            return;
-        }
-
-        // If items are different types
-        if (type1 !== type2) {
-            diffs[key] = item2;
-            return;
-        }
-
-        // If an object, compare recursively
-        if (type1 === '[object Object]') {
-            let objDiff = diff(item1, item2);
-            if (Object.keys(objDiff).length > 1) {
-                diffs[key] = objDiff;
-            }
-            return;
-        }
-
-        // If an array, compare
-        if (type1 === '[object Array]') {
-            if (!arraysMatch(item1, item2)) {
-                diffs[key] = item2;
-            }
-            return;
-        }
-
-        // Else if it's a function, convert to a string and compare
-        // Otherwise, just compare
-        if (type1 === '[object Function]') {
-            if (item1.toString() !== item2.toString()) {
-                diffs[key] = item2;
-            }
-        } else {
-            if (item1 !== item2) {
-                diffs[key] = item2;
-            }
-        }
-
-    };
-
-
-    //
-    // Compare our objects
-    //
-
-    // Loop through the first object
-    for (key in obj1) {
-        if (obj1.hasOwnProperty(key)) {
-            compare(obj1[key], obj2[key], key);
-        }
-    }
-
-    // Loop through the second object and find missing items
-    for (key in obj2) {
-        if (obj2.hasOwnProperty(key)) {
-            if (!obj1[key] && obj1[key] !== obj2[key] ) {
-                diffs[key] = obj2[key];
-            }
-        }
-    }
-
-    // Return the object of differences
-    return diffs;
-
-};

photon-client/src/store/index.js

@@ -52,7 +52,7 @@ export default new Vuex.Store({
                 isFovConfigurable: true,
                 calibrated: false,
                 currentPipelineSettings: {
-                    pipelineType: 4, // One of "calib", "driver", "reflective", "shape", "AprilTag"
+                    pipelineType: 5, // One of "calib", "driver", "reflective", "shape", "AprilTag"
                     // 2 is reflective
 
                     // Settings that apply to all pipeline types
@@ -91,7 +91,7 @@ export default new Vuex.Store({
                     cornerDetectionAccuracyPercentage: 10,
 
                     // Settings that apply to AprilTag
-                    tagFamily: 0,
+                    tagFamily: 1,
                     decimate: 1.0,
                     blur: 0.0,
                     threads: 1,

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;
@@ -676,7 +795,7 @@ export default {
                 console.log("starting calibration with index " + calData.videoModeIndex);
             }
             this.$store.commit('currentPipelineIndex', -2);
-            this.$store.state.websocket.send(this.$msgPack.encode(data));
+            this.$store.state.websocket.ws.send(this.$msgPack.encode(data));
         },
         sendCalibrationFinish() {
             console.log("finishing calibration for index " + this.$store.getters.currentCameraIndex);

photon-client/src/views/LogsView.vue

@@ -12,12 +12,21 @@
         color="secondary"
         style="margin-left: auto;"
         depressed
-        @click="download('photonlog.log', rawLogs.map(it => it.message).join('\n'))"
+        @click="$refs.exportLogFile.click()"
       >
         <v-icon left>
           mdi-download
         </v-icon>
         Download Log
+
+        <!-- Special hidden link that gets 'clicked' when the user exports journalctl logs -->
+        <a
+          ref="exportLogFile"
+          style="color: black; text-decoration: none; display: none"
+          :href="'http://' + this.$address + '/api/settings/photonvision-journalctl.txt'"
+          download="photonvision-journalctl.txt"
+        />
+
       </v-btn>
     </v-card-title>
     <div class="pr-6 pl-6">

photon-client/src/views/PipelineView.vue

@@ -1,75 +1,75 @@
 <template>
   <div>
     <v-container
-      class="pa-3"
-      fluid
+        class="pa-3"
+        fluid
     >
       <v-row
-        no-gutters
-        align="center"
-        justify="center"
+          no-gutters
+          align="center"
+          justify="center"
       >
         <v-col
-          cols="12"
-          :class="['pb-3 ', 'pr-lg-3']"
-          lg="8"
-          align-self="stretch"
+            cols="12"
+            :class="['pb-3 ', 'pr-lg-3']"
+            lg="8"
+            align-self="stretch"
         >
           <v-card
-            color="primary"
-            height="100%"
-            style="display: flex; flex-direction: column"
-            dark
+              color="primary"
+              height="100%"
+              style="display: flex; flex-direction: column"
+              dark
           >
             <v-card-title
-              class="pb-0 mb-0 pl-4 pt-1"
-              style="height: 15%; min-height: 50px;"
+                class="pb-0 mb-0 pl-4 pt-1"
+                style="height: 15%; min-height: 50px;"
             >
               Cameras
               <v-chip
-                :class="fpsTooLow ? 'ml-2 mt-1' : 'mt-2'"
-                x-small
-                label
-                :color="fpsTooLow ? 'error' : 'transparent'"
-                :text-color="fpsTooLow ? 'white' : 'grey'"
+                  :class="fpsTooLow ? 'ml-2 mt-1' : 'mt-2'"
+                  x-small
+                  label
+                  :color="fpsTooLow ? 'error' : 'transparent'"
+                  :text-color="fpsTooLow ? 'white' : 'grey'"
               >
                 <span class="pr-1">Processing @ {{ Math.round($store.state.pipelineResults.fps) }}&nbsp;FPS &ndash;</span>
                 <span v-if="fpsTooLow && !$store.getters.currentPipelineSettings.inputShouldShow && $store.getters.pipelineType == 2">HSV thresholds are too broad; narrow them for better performance</span>
-                <span v-else-if="$fpsTooLow && getters.currentCameraSettings.inputShouldShow">stop viewing the raw stream for better performance</span>
+                <span v-else-if="fpsTooLow && getters.currentCameraSettings.inputShouldShow">stop viewing the raw stream for better performance</span>
                 <span v-else>{{ Math.min(Math.round($store.state.pipelineResults.latency), 9999) }} ms latency</span>
               </v-chip>
               <v-switch
-                v-model="driverMode"
-                label="Driver Mode"
-                style="margin-left: auto;"
-                color="accent"
+                  v-model="driverMode"
+                  label="Driver Mode"
+                  style="margin-left: auto;"
+                  color="accent"
               />
             </v-card-title>
             <v-row
-              align="center"
+                align="center"
             >
               <v-col
-                v-for="idx in (selectedOutputs instanceof Array ? selectedOutputs : [selectedOutputs])"
-                :key="idx"
-                cols="12"
-                :md="selectedOutputs.length === 1 ? 12 : Math.floor(12 / selectedOutputs.length)"
-                class="pb-0 pt-0"
-                style="height: 100%;"
+                  v-for="idx in (selectedOutputs instanceof Array ? selectedOutputs : [selectedOutputs])"
+                  :key="idx"
+                  cols="12"
+                  :md="selectedOutputs.length === 1 ? 12 : Math.floor(12 / selectedOutputs.length)"
+                  class="pb-0 pt-0"
+                  style="height: 100%;"
               >
                 <div style="position: relative; width: 100%; height: 100%;">
                   <cv-image
-                    :id="idx === 0 ? 'raw-stream' : 'processed-stream'"
-                    ref="streams"
-                    :idx=idx
-                    :disconnected="!$store.state.backendConnected"
-                    scale="100"
-                    :max-height="$store.getters.isDriverMode ? '40vh' : '300px'"
-                    :max-height-md="$store.getters.isDriverMode ? '50vh' : '380px'"
-                    :max-height-lg="$store.getters.isDriverMode ? '55vh' : '390px'"
-                    :max-height-xl="$store.getters.isDriverMode ? '60vh' : '450px'"
-                    :alt="'Stream ' + idx"
-                    :color-picking="$store.state.colorPicking && idx === 0"
-                    @click="onImageClick"
+                      :id="idx === 0 ? 'raw-stream' : 'processed-stream'"
+                      ref="streams"
+                      :idx=idx
+                      :disconnected="!$store.state.backendConnected"
+                      scale="100"
+                      :max-height="$store.getters.isDriverMode ? '40vh' : '300px'"
+                      :max-height-md="$store.getters.isDriverMode ? '50vh' : '380px'"
+                      :max-height-lg="$store.getters.isDriverMode ? '55vh' : '390px'"
+                      :max-height-xl="$store.getters.isDriverMode ? '60vh' : '450px'"
+                      :alt="idx === 0 ? 'Raw stream' : 'Processed stream'"
+                      :color-picking="$store.state.colorPicking && idx === 0"
+                      @click="onImageClick"
                   />
                 </div>
               </v-col>
@@ -77,44 +77,44 @@
           </v-card>
         </v-col>
         <v-col
-          cols="12"
-          class="pb-3"
-          lg="4"
-          align-self="stretch"
+            cols="12"
+            class="pb-3"
+            lg="4"
+            align-self="stretch"
         >
           <v-card
-            color="primary"
+              color="primary"
           >
             <camera-and-pipeline-select />
           </v-card>
           <v-card
-            :disabled="$store.getters.isDriverMode || $store.state.colorPicking"
-            class="mt-3"
-            color="primary"
+              :disabled="$store.getters.isDriverMode || $store.state.colorPicking"
+              class="mt-3"
+              color="primary"
           >
             <v-row
-              align="center"
-              class="pl-3 pr-3"
+                align="center"
+                class="pl-3 pr-3"
             >
               <v-col lg="12">
                 <p style="color: white;">
                   Processing mode:
                 </p>
                 <v-btn-toggle
-                  v-model="processingMode"
-                  mandatory
-                  dark
-                  class="fill"
+                    v-model="processingMode"
+                    mandatory
+                    dark
+                    class="fill"
                 >
                   <v-btn
-                    color="secondary"
+                      color="secondary"
                   >
                     <v-icon>mdi-crop-square</v-icon>
                     <span>2D</span>
                   </v-btn>
                   <v-btn
-                    color="secondary"
-                    @click="on3DClick"
+                      color="secondary"
+                      @click="on3DClick"
                   >
                     <v-icon>mdi-cube-outline</v-icon>
                     <span>3D</span>
@@ -126,22 +126,22 @@
                   Stream display:
                 </p>
                 <v-btn-toggle
-                  v-model="selectedOutputs"
-                  :multiple="$vuetify.breakpoint.mdAndUp"
-                  mandatory
-                  dark
-                  class="fill"
+                    v-model="selectedOutputs"
+                    :multiple="$vuetify.breakpoint.mdAndUp"
+                    mandatory
+                    dark
+                    class="fill"
                 >
                   <v-btn
-                    color="secondary"
-                    class="fill"
+                      color="secondary"
+                      class="fill"
                   >
                     <v-icon>mdi-import</v-icon>
                     <span>Raw</span>
                   </v-btn>
                   <v-btn
-                    color="secondary"
-                    class="fill"
+                      color="secondary"
+                      class="fill"
                   >
                     <v-icon>mdi-export</v-icon>
                     <span>Processed</span>
@@ -154,29 +154,29 @@
       </v-row>
       <v-row no-gutters>
         <v-col
-          v-for="(tabs, idx) in tabGroups"
-          :key="idx"
-          :cols="Math.floor(12 / tabGroups.length)"
-          :class="idx !== tabGroups.length - 1 ? 'pr-3' : ''"
-          align-self="stretch"
+            v-for="(tabs, idx) in tabGroups"
+            :key="idx"
+            :cols="Math.floor(12 / tabGroups.length)"
+            :class="idx !== tabGroups.length - 1 ? 'pr-3' : ''"
+            align-self="stretch"
         >
           <v-card
-            color="primary"
-            height="100%"
-            class="pr-4 pl-4"
+              color="primary"
+              height="100%"
+              class="pr-4 pl-4"
           >
             <v-tabs
-              v-if="!$store.getters.isDriverMode"
-              v-model="selectedTabs[idx]"
-              grow
-              background-color="primary"
-              dark
-              height="48"
-              slider-color="accent"
+                v-if="!$store.getters.isDriverMode"
+                v-model="selectedTabs[idx]"
+                grow
+                background-color="primary"
+                dark
+                height="48"
+                slider-color="accent"
             >
               <v-tab
-                v-for="(tab, i) in tabs"
-                :key="i"
+                  v-for="(tab, i) in tabs"
+                  :key="i"
               >
                 {{ tab.name }}
               </v-tab>
@@ -184,10 +184,10 @@
             <div class="pl-4 pr-4 pt-2">
               <keep-alive>
                 <component
-                  :is="(tabs[selectedTabs[idx]] || tabs[0]).component"
-                  :ref="(tabs[selectedTabs[idx]] || tabs[0]).name"
-                  v-model="$store.getters.pipeline"
-                  @update="$emit('save')"
+                    :is="(tabs[selectedTabs[idx]] || tabs[0]).component"
+                    :ref="(tabs[selectedTabs[idx]] || tabs[0]).name"
+                    v-model="$store.getters.pipeline"
+                    @update="$emit('save')"
                 />
               </keep-alive>
             </div>
@@ -197,18 +197,18 @@
     </v-container>
 
     <v-snackbar
-      v-model="showNTWarning"
-      color="error"
-      timeout="-1"
-      top
+        v-model="showNTWarning"
+        color="error"
+        timeout="-1"
+        top
     >
       {{ $store.state.settings.networkSettings.runNTServer ?
         "NetworkTables server enabled! PhotonLib may not work." :
         "NetworkTables not connected! Are you on a network with a robot?" }}
       <template v-slot:action>
         <v-btn
-          text
-          @click="hideNTWarning = true"
+            text
+            @click="hideNTWarning = true"
         >
           Hide
         </v-btn>
@@ -216,12 +216,12 @@
     </v-snackbar>
 
     <v-dialog
-      v-model="dialog"
-      width="500"
+        v-model="dialog"
+        width="500"
     >
       <v-card
-        color="primary"
-        dark
+          color="primary"
+          dark
       >
         <v-card-title>
           Current resolution not calibrated
@@ -230,9 +230,9 @@
         <v-card-text>
           Because the current resolution {{ this.$store.getters.currentVideoFormat.width }} x {{ this.$store.getters.currentVideoFormat.height }} is not yet calibrated, 3D mode cannot be enabled. Please
           <a
-            href="/#/cameras"
-            class="white--text"
-            @click="$emit('switch-to-cameras')"
+              href="/#/cameras"
+              class="white--text"
+              @click="$emit('switch-to-cameras')"
           > visit the Cameras tab</a> to calibrate this resolution. For now, SolvePNP will do nothing.
         </v-card-text>
 
@@ -241,9 +241,9 @@
         <v-card-actions>
           <v-spacer />
           <v-btn
-            color="white"
-            text
-            @click="closeUncalibratedDialog"
+              color="white"
+              text
+              @click="closeUncalibratedDialog"
           >
             OK
           </v-btn>
@@ -264,252 +264,264 @@ import TargetsTab from "./PipelineViews/TargetsTab";
 import Map3DTab from './PipelineViews/Map3DTab';
 import PnPTab from './PipelineViews/PnPTab';
 import AprilTagTab from './PipelineViews/AprilTagTab';
+import ArucoTab from './PipelineViews/ArucoTab';
 
 export default {
-    name: 'Pipeline',
-    components: {
-        CameraAndPipelineSelect,
-        cvImage,
-        InputTab,
-        ThresholdTab,
-        ContoursTab,
-        OutputTab,
-        TargetsTab,
-        Map3DTab,
-        PnPTab,
-        AprilTagTab,
-    },
-    data() {
-        return {
-            selectedTabsData: [0, 0, 0, 0],
-            counterData: 0,
-            dialog: false,
-            processingModeOverride: false,
-            hideNTWarning: false,
-        }
-    },
-    computed: {
-        selectedTabs: {
-            get() {
-                return this.$store.getters.isDriverMode ? [0] : this.selectedTabsData;
-            },
-            set(value) {
-                this.selectedTabsData = value;
-            }
-        },
-        tabGroups: {
-            get() {
-                let tabs = {
-                    input: {
-                        name: "Input",
-                        component: "InputTab",
-                    },
-                    threshold: {
-                        name: "Threshold",
-                        component: "ThresholdTab",
-                    },
-                    contours: {
-                        name: "Contours",
-                        component: "ContoursTab",
-                    },
-                    apriltag: {
-                        name: "AprilTag",
-                        component: "AprilTagTab",
-                    },
-                    output: {
-                        name: "Output",
-                        component: "OutputTab",
-                    },
-                    targets: {
-                        name: "Targets",
-                        component: "TargetsTab",
-                    },
-                    pnp: {
-                        name: "PnP",
-                        component: "PnPTab",
-                    },
-                    map3d: {
-                        name: "3D",
-                        component: "Map3DTab",
-                    }
-                };
-
-                // If not in 3d, name "3D" is illegal
-                const allow3d = this.$store.getters.currentPipelineSettings.solvePNPEnabled;
-                // If in apriltag, "Threshold" and "Contours" are illegal -- otherwise "AprilTag" is
-                const isAprilTag = (this.$store.getters.currentPipelineSettings.pipelineType - 2) === 2;
-
-                // 2D array of tab names and component names; each sub-array is a separate tab group
-                let ret = [];
-                if (this.$vuetify.breakpoint.smAndDown || this.$store.getters.isDriverMode || (this.$vuetify.breakpoint.mdAndDown && !this.$store.state.compactMode)) {
-                    // One big tab group with all the tabs
-                    ret[0] = Object.values(tabs);
-                } else if (this.$vuetify.breakpoint.mdAndDown || !this.$store.state.compactMode) {
-                    // Two tab groups, one with "input, threshold, contours, output" and the other with "target info, 3D"
-                    ret[0] = [tabs.input, tabs.threshold, tabs.contours, tabs.apriltag, tabs.output];
-                    ret[1] = [tabs.targets, tabs.pnp, tabs.map3d];
-                } else if (this.$vuetify.breakpoint.lgAndDown) {
-                    // Three tab groups, one with "input", one with "threshold, contours, output", and the other with "target info, 3D"
-                    ret[0] = [tabs.input];
-                    ret[1] = [tabs.threshold, tabs.contours, tabs.apriltag, tabs.output];
-                    ret[2] = [tabs.targets, tabs.pnp, tabs.map3d];
-                } else if (this.$vuetify.breakpoint.xl) {
-                    // Three tab groups, one with "input", one with "threshold, contours", and the other with "output, target info, 3D"
-                    ret[0] = [tabs.input];
-                    ret[1] = [tabs.threshold];
-                    ret[2] = [tabs.contours, tabs.apriltag, tabs.output];
-                    ret[3] = [tabs.targets, tabs.pnp, tabs.map3d];
-                }
-
-                for(let i = 0; i < ret.length; i++) {
-                  const group = ret[i];
-
-                  // All the tabs we allow
-                  const filteredGroup = group.filter(it =>
-                          !(!allow3d && it.name === "3D") //Filter out 3D tab any time 3D isn't calibrated
-                       && !((!allow3d || isAprilTag) && it.name === "PnP") //Filter out the PnP config tab if 3D isn't available, or we're doing Apriltags
-                       && !(isAprilTag && (it.name === "Threshold")) //Filter out threshold tab if we're doing apriltags
-                       && !(isAprilTag && (it.name === "Contours")) //Filter out contours if we're doing Apriltag
-                       && !(!isAprilTag && it.name === "AprilTag") //Filter out apriltag unless we actually are doing Apriltags
-                      );
-                  ret[i] = filteredGroup;
-                }
-
-                // One last filter to remove empty lists
-                return ret.filter(it => it !== undefined && it.length > 0);
-            }
-        },
-        processingMode: {
-            get() {
-                return (this.$store.getters.currentPipelineSettings.solvePNPEnabled || this.processingModeOverride) ? 1 : 0;
-            },
-            set(value) {
-                if (this.$store.getters.isCalibrated) {
-                    this.$store.getters.currentPipelineSettings.solvePNPEnabled = value === 1;
-                    this.handlePipelineUpdate("solvePNPEnabled", value === 1);
-                }
-            }
-        },
-        driverMode: {
-            get() {
-                return this.$store.getters.isDriverMode;
-            },
-            set(value) {
-                this.$store.getters.currentCameraSettings.currentPipelineIndex = value ? -1 : 0;
-                this.handleInputWithIndex('currentPipeline', value ? -1 : 0);
-            }
-        },
-        selectedOutputs: {
-            // All this logic exists to deal with the reality that the output select buttons sometimes need an array and sometimes need a number (depending on whether or not they're exclusive)
-            get() {
-                // We switch the selector to single-select only on sm-and-down size devices, so we have to return a Number instead of an Array in that state
-                let ret = [];
-                if (this.$store.state.colorPicking) {
-                    ret = [0]; // We want the input stream only while color picking
-                } else if (this.$store.getters.isDriverMode) {
-                    ret = [1]; // We want only the output stream in driver mode
-                } else {
-                  if (this.$store.getters.currentPipelineSettings.inputShouldShow) ret = ret.concat([0]);
-                  if (this.$store.getters.currentPipelineSettings.outputShouldShow) ret = ret.concat([1]);
-                  if (!ret.length) ret = [0];
-                }
-
-                if (this.$vuetify.breakpoint.mdAndUp) {
-                    return ret;
-                } else {
-                    return ret[0] || 0;
-                }
-            },
-            set(value) {
-                let valToCommit = [0];
-                if (value instanceof Array) {
-                  // Value is already an array, we don't need to do anything
-                  valToCommit = value;
-                } else if (value) {
-                  // Value is assumed to be a number, so we wrap it into an array
-                  valToCommit = [value];
-                }
-
-                this.$store.commit("mutatePipeline", {"inputShouldShow": valToCommit.includes(0)});
-                this.$store.commit("mutatePipeline", {"outputShouldShow": valToCommit.includes(1)});
-                this.handlePipelineUpdate("inputShouldShow", valToCommit.includes(0));
-            }
-        },
-        fpsTooLow: {
-            get() {
-                // For now we only show the FPS is too low warning when GPU acceleration is enabled, because we don't really trust the presented video modes otherwise
-                return this.$store.state.pipelineResults.fps - this.$store.getters.currentVideoFormat.fps < -5 && this.$store.state.pipelineResults.fps !== 0 && !this.$store.getters.isDriverMode && this.$store.state.settings.general.gpuAcceleration;
-            }
-        },
-        latency: {
-            get() {
-                return this.$store.getters.currentPipelineResults.latency;
-            }
-        },
-        isCalibrated: {
-          get() {
-            const resolution = this.$store.getters.videoFormatList[this.$store.getters.currentPipelineSettings.cameraVideoModeIndex];
-            return this.$store.getters.currentCameraSettings.calibrations
-                    .some(e => e.width === resolution.width && e.height === resolution.height)
-          }
-        },
-        isRobotConnected: {
-          get() {
-            // return this.$store.state.ntConnectionInfo.connected && this.$store.state.backendConnected;
-            return true;
-          }
-        },
-        showNTWarning: {
-          get() {
-            return (!this.$store.state.ntConnectionInfo.connected || this.$store.state.settings.networkSettings.runNTServer) && this.$store.state.settings.networkSettings.teamNumber > 0 && this.$store.state.backendConnected && !this.hideNTWarning;
-          }
-        },
-    },
-    created() {
-        this.$store.state.connectedCallbacks.push(this.reloadStreams)
-    },
-    methods: {
-        reloadStreams() {
-            // Reload the streams as we technically close and reopen them
-            this.$refs.streams.forEach(it => it.reload())
-        },
-        onImageClick(event) {
-            // Only run on the input stream
-            if (event.target.alt !== "Stream0") return;
-            // Get a reference to the threshold tab (if it is shown) and call its "onClick" method
-            let ref = this.$refs["Threshold"];
-            if (ref && ref[0])
-                ref[0].onClick(event)
-        },
-        on3DClick() {
-            if (!this.$store.getters.isCalibrated) {
-                this.dialog = true;
-                this.processingModeOverride = true;
-            }
-        },
-        closeUncalibratedDialog() {
-            this.dialog = false;
-            this.processingModeOverride = false;
-            // this.$store.getters.currentPipelineSettings.solvePNPEnabled = false;
-            this.handlePipelineUpdate("solvePNPEnabled", false);
-        }
+  name: 'Pipeline',
+  components: {
+    CameraAndPipelineSelect,
+    cvImage,
+    InputTab,
+    ThresholdTab,
+    ContoursTab,
+    OutputTab,
+    TargetsTab,
+    Map3DTab,
+    PnPTab,
+    AprilTagTab,
+    ArucoTab,
+  },
+  data() {
+    return {
+      selectedTabsData: [0, 0, 0, 0],
+      counterData: 0,
+      dialog: false,
+      processingModeOverride: false,
+      hideNTWarning: false,
     }
+  },
+  computed: {
+    selectedTabs: {
+      get() {
+        return this.$store.getters.isDriverMode ? [0] : this.selectedTabsData;
+      },
+      set(value) {
+        this.selectedTabsData = value;
+      }
+    },
+    tabGroups: {
+      get() {
+        let tabs = {
+          input: {
+            name: "Input",
+            component: "InputTab",
+          },
+          threshold: {
+            name: "Threshold",
+            component: "ThresholdTab",
+          },
+          contours: {
+            name: "Contours",
+            component: "ContoursTab",
+          },
+          apriltag: {
+            name: "AprilTag",
+            component: "AprilTagTab",
+          },
+          aruco: {
+            name: "Aruco",
+            component: "ArucoTab",
+          },
+          output: {
+            name: "Output",
+            component: "OutputTab",
+          },
+          targets: {
+            name: "Targets",
+            component: "TargetsTab",
+          },
+          pnp: {
+            name: "PnP",
+            component: "PnPTab",
+          },
+          map3d: {
+            name: "3D",
+            component: "Map3DTab",
+          }
+        };
+
+        // If not in 3d, name "3D" is illegal
+        const allow3d = this.$store.getters.currentPipelineSettings.solvePNPEnabled;
+        // If in apriltag, "Threshold" and "Contours" are illegal -- otherwise "AprilTag" is
+        const isAprilTag = (this.$store.getters.currentPipelineSettings.pipelineType - 2) === 2;
+        const isAruco = (this.$store.getters.currentPipelineSettings.pipelineType - 2) === 3;
+
+        // 2D array of tab names and component names; each sub-array is a separate tab group
+        let ret = [];
+        if (this.$vuetify.breakpoint.smAndDown || this.$store.getters.isDriverMode || (this.$vuetify.breakpoint.mdAndDown && !this.$store.state.compactMode)) {
+          // One big tab group with all the tabs
+          ret[0] = Object.values(tabs);
+        } else if (this.$vuetify.breakpoint.mdAndDown || !this.$store.state.compactMode) {
+          // Two tab groups, one with "input, threshold, contours, output" and the other with "target info, 3D"
+          ret[0] = [tabs.input, tabs.threshold, tabs.contours, tabs.apriltag, tabs.aruco, tabs.output];
+          ret[1] = [tabs.targets, tabs.pnp, tabs.map3d];
+        } else if (this.$vuetify.breakpoint.lgAndDown) {
+          // Three tab groups, one with "input", one with "threshold, contours, output", and the other with "target info, 3D"
+          ret[0] = [tabs.input];
+          ret[1] = [tabs.threshold, tabs.contours, tabs.apriltag,tabs.aruco, tabs.output];
+          ret[2] = [tabs.targets, tabs.pnp, tabs.map3d];
+        } else if (this.$vuetify.breakpoint.xl) {
+          // Three tab groups, one with "input", one with "threshold, contours", and the other with "output, target info, 3D"
+          ret[0] = [tabs.input];
+          ret[1] = [tabs.threshold];
+          ret[2] = [tabs.contours, tabs.apriltag, tabs.aruco,tabs.output];
+          ret[3] = [tabs.targets, tabs.pnp, tabs.map3d];
+        }
+
+        for(let i = 0; i < ret.length; i++) {
+          const group = ret[i];
+
+          // All the tabs we allow
+          const filteredGroup = group.filter(it =>
+              !(!allow3d && it.name === "3D") //Filter out 3D tab any time 3D isn't calibrated
+              && !((!allow3d || isAprilTag || isAruco) && it.name === "PnP") //Filter out the PnP config tab if 3D isn't available, or we're doing Apriltags
+              && !((isAprilTag || isAruco) && (it.name === "Threshold")) //Filter out threshold tab if we're doing apriltags
+              && !((isAprilTag || isAruco)&& (it.name === "Contours")) //Filter out contours if we're doing Apriltag
+              && !(!isAprilTag && it.name === "AprilTag") //Filter out apriltag unless we actually are doing Apriltags
+              && !(!isAruco && it.name === "Aruco")
+          );
+          ret[i] = filteredGroup;
+        }
+
+        // One last filter to remove empty lists
+        return ret.filter(it => it !== undefined && it.length > 0);
+      }
+    },
+    processingMode: {
+      get() {
+        return (this.$store.getters.currentPipelineSettings.solvePNPEnabled || this.processingModeOverride) ? 1 : 0;
+      },
+      set(value) {
+        if (this.$store.getters.isCalibrated) {
+          this.$store.getters.currentPipelineSettings.solvePNPEnabled = value === 1;
+          this.handlePipelineUpdate("solvePNPEnabled", value === 1);
+        }
+      }
+    },
+    driverMode: {
+      get() {
+        return this.$store.getters.isDriverMode;
+      },
+      set(value) {
+        this.$store.getters.currentCameraSettings.currentPipelineIndex = value ? -1 : 0;
+        this.handleInputWithIndex('currentPipeline', value ? -1 : 0);
+      }
+    },
+    selectedOutputs: {
+      // All this logic exists to deal with the reality that the output select buttons sometimes need an array and sometimes need a number (depending on whether or not they're exclusive)
+      get() {
+        // We switch the selector to single-select only on sm-and-down size devices, so we have to return a Number instead of an Array in that state
+        let ret = [];
+        if (this.$store.state.colorPicking) {
+          ret = [0]; // We want the input stream only while color picking
+        } else if (this.$store.getters.isDriverMode) {
+          ret = [1]; // We want only the output stream in driver mode
+        } else {
+          if (this.$store.getters.currentPipelineSettings.inputShouldShow) ret = ret.concat([0]);
+          if (this.$store.getters.currentPipelineSettings.outputShouldShow) ret = ret.concat([1]);
+          if (!ret.length) ret = [0];
+        }
+
+        if (this.$vuetify.breakpoint.mdAndUp) {
+          return ret;
+        } else {
+          return ret[0] || 0;
+        }
+      },
+      set(value) {
+        let valToCommit = [0];
+        if (value instanceof Array) {
+          // Value is already an array, we don't need to do anything
+          valToCommit = value;
+        } else if (value) {
+          // Value is assumed to be a number, so we wrap it into an array
+          valToCommit = [value];
+        }
+
+        this.$store.commit("mutatePipeline", {"inputShouldShow": valToCommit.includes(0)});
+        this.$store.commit("mutatePipeline", {"outputShouldShow": valToCommit.includes(1)});
+        this.handlePipelineUpdate("inputShouldShow", valToCommit.includes(0));
+      }
+    },
+    fpsTooLow: {
+      get() {
+        // For now we only show the FPS is too low warning when GPU acceleration is enabled, because we don't really trust the presented video modes otherwise
+        const currFPS = this.$store.state.pipelineResults.fps;
+        const targetFPS = this.$store.getters.currentVideoFormat.fps;
+        const driverMode = this.$store.getters.isDriverMode;
+        const gpuAccel = this.$store.state.settings.general.gpuAcceleration === true;
+        const isReflective = this.$store.getters.pipelineType === 2;
+
+        return (currFPS - targetFPS) < -5 && this.$store.state.pipelineResults.fps !== 0 && !driverMode && gpuAccel && isReflective;
+      }
+    },
+    latency: {
+      get() {
+        return this.$store.getters.currentPipelineResults.latency;
+      }
+    },
+    isCalibrated: {
+      get() {
+        const resolution = this.$store.getters.videoFormatList[this.$store.getters.currentPipelineSettings.cameraVideoModeIndex];
+        return this.$store.getters.currentCameraSettings.calibrations
+            .some(e => e.width === resolution.width && e.height === resolution.height)
+      }
+    },
+    isRobotConnected: {
+      get() {
+        // return this.$store.state.ntConnectionInfo.connected && this.$store.state.backendConnected;
+        return true;
+      }
+    },
+    showNTWarning: {
+      get() {
+        return (!this.$store.state.ntConnectionInfo.connected || this.$store.state.settings.networkSettings.runNTServer) && this.$store.state.settings.networkSettings.teamNumber > 0 && this.$store.state.backendConnected && !this.hideNTWarning;
+      }
+    },
+  },
+  created() {
+    this.$store.state.connectedCallbacks.push(this.reloadStreams)
+  },
+  methods: {
+    reloadStreams() {
+      // Reload the streams as we technically close and reopen them
+      this.$refs.streams.forEach(it => it.reload())
+    },
+    onImageClick(event) {
+      // Get a reference to the threshold tab (if it is shown) and call its "onClick" method
+      let ref = this.$refs["Threshold"];
+      if (ref && ref[0])
+        ref[0].onClick(event)
+    },
+    on3DClick() {
+      if (!this.$store.getters.isCalibrated) {
+        this.dialog = true;
+        this.processingModeOverride = true;
+      }
+    },
+    closeUncalibratedDialog() {
+      this.dialog = false;
+      this.processingModeOverride = false;
+      // this.$store.getters.currentPipelineSettings.solvePNPEnabled = false;
+      this.handlePipelineUpdate("solvePNPEnabled", false);
+    }
+  }
 }
 </script>
 
 <style scoped>
 .v-btn-toggle.fill {
-    width: 100%;
-    height: 100%;
+  width: 100%;
+  height: 100%;
 }
 
 .v-btn-toggle.fill > .v-btn {
-    width: 50%;
-    height: 100%;
+  width: 50%;
+  height: 100%;
 }
 
 th {
-    width: 80px;
-    text-align: center;
+  width: 80px;
+  text-align: center;
 }
 </style>

photon-client/src/views/PipelineViews/AprilTagTab.vue

@@ -1,196 +1,154 @@
 <template>
   <div>
-    <v-select
-      v-model="selectedFamily"
-      dark
-      color="accent"
-      item-color="secondary"
-      label="Select target family"
-      :items="familyList"
-      @input="handlePipelineUpdate('tagFamily', familyList.indexOf(selectedFamily))"
-    />
-    <v-select
-      v-model="selectedModel"
-      dark
-      color="accent"
-      item-color="secondary"
-      label="Select a target model"
-      :items="targetList"
-      item-text="name"
-      item-value="data"
-      @input="handlePipelineUpdate('targetModel', targetList.indexOf(selectedModel) + 6)"
+    <CVselect
+        v-model="selectedFamily"
+        name="Target family"
+        :list="['AprilTag family 36h11', 'AprilTag family 25h9', 'AprilTag family 16h5']"
+        select-cols="8"
+        @input="handlePipelineUpdate('tagFamily', selectedFamily)"
     />
     <CVslider
-      v-model="decimate"
-      class="pt-2"
-      slider-cols="8"
-      name="Decimate"
-      min="1"
-      max="8"
-      step="1.0"
-      tooltip="Increases FPS at the expense of range by reducing image resolution initially"
-      @input="handlePipelineData('decimate')"
+        v-model="decimate"
+        class="pt-2"
+        slider-cols="8"
+        name="Decimate"
+        min="1"
+        max="8"
+        step="1.0"
+        tooltip="Increases FPS at the expense of range by reducing image resolution initially"
+        @input="handlePipelineData('decimate')"
     />
     <CVslider
-      v-model="blur"
-      class="pt-2"
-      slider-cols="8"
-      name="Blur"
-      min="0"
-      max="5"
-      step=".01"
-      tooltip="Gaussian blur added to the image, high FPS cost for slightly decreased noise"
-      @input="handlePipelineData('blur')"
+        v-model="blur"
+        class="pt-2"
+        slider-cols="8"
+        name="Blur"
+        min="0"
+        max="5"
+        step=".01"
+        tooltip="Gaussian blur added to the image, high FPS cost for slightly decreased noise"
+        @input="handlePipelineData('blur')"
     />
     <CVslider
-      v-model="threads"
-      class="pt-2"
-      slider-cols="8"
-      name="Threads"
-      min="1"
-      max="8"
-      step="1"
-      tooltip="Number of threads spawned by the AprilTag detector"
-      @input="handlePipelineData('threads')"
+        v-model="threads"
+        class="pt-2"
+        slider-cols="8"
+        name="Threads"
+        min="1"
+        max="8"
+        step="1"
+        tooltip="Number of threads spawned by the AprilTag detector"
+        @input="handlePipelineData('threads')"
     />
     <CVswitch
-      v-model="refineEdges"
-      class="pt-2"
-      slider-cols="8"
-      name="Refine Edges"
-      tooltip="Further refines the apriltag corner position initial estimate, suggested left on"
-      @input="handlePipelineData('refineEdges')"
+        v-model="refineEdges"
+        class="pt-2"
+        slider-cols="8"
+        name="Refine Edges"
+        tooltip="Further refines the apriltag corner position initial estimate, suggested left on"
+        @input="handlePipelineData('refineEdges')"
     />
     <CVslider
-      v-model="hammingDist"
-      class="pt-2 pb-4"
-      slider-cols="8"
-      name="Max error bits"
-      min="0"
-      max="10"
-      step="1"
-      tooltip="Maximum number of error bits to correct; potential tags with more will be thrown out. For smaller tags (like 16h5), set this as low as possible."
-      @input="handlePipelineData('hammingDist')"
+        v-model="decisionMargin"
+        class="pt-2 pb-4"
+        slider-cols="8"
+        name="Decision Margin Cutoff"
+        min="0"
+        max="250"
+        step="1"
+        tooltip="Tags with a 'margin' (decoding quality score) less than this wil be rejected. Increase this to reduce the number of false positive detections"
+        @input="handlePipelineData('decisionMargin')"
     />
     <CVslider
-      v-model="decisionMargin"
-      class="pt-2 pb-4"
-      slider-cols="8"
-      name="Decision Margin Cutoff"
-      min="0"
-      max="250"
-      step="1"
-      tooltip="Tags with a 'margin' (decoding quality score) less than this wil be rejected. Increase this to reduce the number of false positive detections"
-      @input="handlePipelineData('decisionMargin')"
-    />
-    <CVslider
-      v-model="numIterations"
-      class="pt-2 pb-4"
-      slider-cols="8"
-      name="Pose Estimation Iterations"
-      min="0"
-      max="500"
-      step="1"
-      tooltip="Number of iterations the pose estimation algorithm will run, 50-100 is a good starting point"
-      @input="handlePipelineData('numIterations')"
+        v-model="numIterations"
+        class="pt-2 pb-4"
+        slider-cols="8"
+        name="Pose Estimation Iterations"
+        min="0"
+        max="500"
+        step="1"
+        tooltip="Number of iterations the pose estimation algorithm will run, 50-100 is a good starting point"
+        @input="handlePipelineData('numIterations')"
     />
   </div>
 </template>
 
 <script>
-    import CVslider from '../../components/common/cv-slider'
-    import CVswitch from '../../components/common/cv-switch'
+import CVslider from '../../components/common/cv-slider'
+import CVswitch from '../../components/common/cv-switch'
+import CVselect from '../../components/common/cv-select'
 
-    export default {
-        name: "AprilTag",
-        components: {
-          CVslider,
-          CVswitch,
-        },
-        data() {
-            return {
-              familyList: ["tag36h11", "tag25h9", "tag16h5"],
-              // Selected model is offset (ew) by 6 from the photon ordinal, as we only wanna show the 36h11 and 16h5 options
-              targetList: ['6.5in (36h11) AprilTag', '6in (16h5) AprilTag'], //Keep in sync with TargetModel.java
-            }
-        },
-        computed: {
-          selectedModel: {
-              get() {
-                  let ret = this.$store.getters.currentPipelineSettings.targetModel - 6
-                  return this.targetList[ret];
-              },
-              set(val) {
-                  this.$store.commit("mutatePipeline", {"targetModel": this.targetList.indexOf(val) + 6})
-              }
-          },
-          selectedFamily: {
-            get() {
-                let ret = this.$store.getters.currentPipelineSettings.tagFamily
-                return this.familyList[ret];
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"tagFamily": this.familyList.indexOf(val)})
-            }
-          },
-          decimate: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.decimate
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"decimate": val});
-            }
-          },
-          hammingDist: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.hammingDist
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"hammingDist": val});
-            }
-          },
-          decisionMargin: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.decisionMargin
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"decisionMargin": val});
-            }
-          },
-          numIterations: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.numIterations
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"numIterations": val});
-            }
-          },
-          blur: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.blur
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"blur": val});
-            }
-          },
-          threads: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.threads
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"threads": val});
-            }
-          },
-          refineEdges: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.refineEdges
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"refineEdges": val});
-            }
-          },
-        },
-        methods: {
-        }
+export default {
+  name: "AprilTag",
+  components: {
+    CVslider,
+    CVswitch,
+    CVselect,
+  },
+  data() {
+    return {
+      familyList: ["AprilTag family 36h11", "AprilTag family 25h9", "AprilTag family 16h5"],
     }
+  },
+  computed: {
+    selectedFamily: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.tagFamily
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"tagFamily": val})
+      }
+    },
+    decimate: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.decimate
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"decimate": val});
+      }
+    },
+    decisionMargin: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.decisionMargin
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"decisionMargin": val});
+      }
+    },
+    numIterations: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.numIterations
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"numIterations": val});
+      }
+    },
+    blur: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.blur
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"blur": val});
+      }
+    },
+    threads: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.threads
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"threads": val});
+      }
+    },
+    refineEdges: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.refineEdges
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"refineEdges": val});
+      }
+    },
+  },
+  methods: {
+  }
+}
 </script>

photon-client/src/views/PipelineViews/ArucoTab.vue

@@ -0,0 +1,76 @@
+<template>
+  <div>
+    <CVslider
+        v-model="decimate"
+        class="pt-2"
+        slider-cols="8"
+        name="Decimate"
+        min="1"
+        max="8"
+        step=".5"
+        tooltip="Increases FPS at the expense of range by reducing image resolution initially"
+        @input="handlePipelineData('decimate')"
+    />
+    <CVslider
+        v-model="numIterations"
+        class="pt-2"
+        slider-cols="8"
+        name="Corner Iterations"
+        min="30"
+        max="1000"
+        step="5"
+        tooltip="How many iterations are going to be used in order to refine corners. Higher values are lead to more accuracy at the cost of performance"
+        @input="handlePipelineData('numIterations')"
+    />
+    <CVslider
+        v-model="cornerAccuracy"
+        class="pt-2"
+        slider-cols="8"
+        name="Corner Accuracy"
+        min=".01"
+        max="100"
+        step=".01"
+        tooltip="Minimum accuracy for the corners, lower is better but more performance intensive "
+        @input="handlePipelineData('cornerAccuracy')"
+    />
+  </div>
+</template>
+
+<script>
+import CVslider from '../../components/common/cv-slider'
+
+export default {
+  name: "Aruco",
+  components: {
+    CVslider
+  },
+  computed: {
+    decimate: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.decimate
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"decimate": val});
+      },
+    },
+      numIterations: {
+        get() {
+          return this.$store.getters.currentPipelineSettings.numIterations
+        },
+        set(val) {
+          this.$store.commit("mutatePipeline", {"numIterations": val});
+        },
+      },
+        cornerAccuracy: {
+          get() {
+            return this.$store.getters.currentPipelineSettings.cornerAccuracy
+          },
+          set(val) {
+            this.$store.commit("mutatePipeline", {"cornerAccuracy": val});
+          },
+        },
+    },
+    methods: {
+    }
+  }
+</script>

photon-client/src/views/PipelineViews/InputTab.vue

@@ -62,11 +62,13 @@
       @input="handlePipelineData('cameraBlueGain')"
       @rollback="e => rollback('cameraBlueGain', e)"
     />
+    <!-- TODO: stop filtering out the 90 degree rotation modes when we fix those in libcamera -->
     <CVselect
       v-model="inputImageRotationMode"
       name="Orientation"
       tooltip="Rotates the camera stream"
       :list="['Normal','90° CW','180°','90° CCW']"
+      :filtered-indices="this.$store.state.settings.general.gpuAcceleration ? new Set([1, 3]) : undefined"
       :select-cols="largeBox"
       @input="handlePipelineData('inputImageRotationMode')"
       @rollback="e => rollback('inputImageRotationMode',e)"

photon-client/src/views/PipelineViews/OutputTab.vue

@@ -10,6 +10,7 @@
     />
 
     <CVselect
+      v-if="!isTagPipeline"
       v-model="contourTargetOrientation"
       name="Target Orientation"
       tooltip="Used to determine how to calculate target landmarks (e.g. the top, left, or bottom of the target)"
@@ -21,7 +22,8 @@
     <CVswitch
       v-model="outputShowMultipleTargets"
       name="Show Multiple Targets"
-      tooltip="If enabled, up to five targets will be displayed and sent to user code"
+      tooltip="If enabled, up to five targets will be displayed and sent to user code, instead of just one"
+      :disabled="isTagPipeline"
       class="mb-4"
       text-cols="3"
       @input="handlePipelineData('outputShowMultipleTargets')"
@@ -137,6 +139,11 @@
                 get() {
                     return undefined; // TODO fix
                 }
+            },
+            isTagPipeline: {
+                get() {
+                     return this.$store.getters.currentPipelineSettings.pipelineType > 3;
+                }
             }
         },
         methods: {

photon-client/src/views/PipelineViews/TargetsTab.vue

@@ -19,7 +19,7 @@
                 Target
               </th>
               <th
-                v-if="$store.getters.pipelineType === 4"
+                  v-if="$store.getters.pipelineType === 4 || (($store.getters.pipelineType - 2) === 3)"
                 class="text-center"
               >
                 Fiducial ID
@@ -62,7 +62,7 @@
               :key="index"
             >
               <td>{{ index }}</td>
-              <td v-if="$store.getters.pipelineType === 4">
+              <td v-if="$store.getters.pipelineType === 4 || (($store.getters.pipelineType - 2) === 3)">
                 {{ parseInt(value.fiducialId) }}
               </td>
               <template v-if="!$store.getters.currentPipelineSettings.solvePNPEnabled">

photon-client/src/views/PipelineViews/ThresholdTab.vue

@@ -247,7 +247,7 @@ export default {
             'cameraIndex': this.$store.state.currentCameraIndex
           }
         });
-        this.$store.state.websocket.send(msg);
+        this.$store.state.websocket.ws.send(msg);
         this.$emit('update');
       }
     },

photon-client/src/views/SettingsView.vue

@@ -37,7 +37,8 @@
     import Networking from './SettingsViews/Networking'
     import Lighting from "./SettingsViews/Lighting";
     import cvImage from '../components/common/cv-image'
-    import General from "./SettingsViews/General";
+    import Stats from "./SettingsViews/Stats";
+    import DeviceControl from "./SettingsViews/DeviceControl";
 
     export default {
         name: 'SettingsTab',
@@ -69,7 +70,7 @@
             },
             tabList: {
                 get() {
-                    return [General, Networking].concat(this.$store.state.settings.lighting.supported ? Lighting : []);
+                    return [Stats, DeviceControl, Networking].concat(this.$store.state.settings.lighting.supported ? Lighting : []);
                 }
             }
         },

photon-client/src/views/SettingsViews/DeviceControl.vue

@@ -0,0 +1,289 @@
+<template>
+  <div>
+    <v-row>
+      <v-col cols="12" lg="4" md="6">
+        <v-btn color="red" @click="restartProgram()">
+          <v-icon left>
+            mdi-restart
+          </v-icon>
+          Restart PhotonVision
+        </v-btn>
+      </v-col>
+      <v-col cols="12" lg="4" md="6">
+        <v-btn color="red" @click="restartDevice()">
+          <v-icon left>
+            mdi-restart-alert
+          </v-icon>
+          Restart Device
+        </v-btn>
+      </v-col>
+      <v-col cols="12" lg="4">
+        <v-btn color="secondary" @click="$refs.offlineUpdate.click()">
+          <v-icon left>
+            mdi-update
+          </v-icon>
+          Offline Update
+        </v-btn>
+      </v-col>
+    </v-row>
+    <v-divider />
+    <v-row>
+      <v-col cols="12" sm="6">
+        <v-btn color="secondary" @click="$refs.exportSettings.click()">
+          <v-icon left>
+            mdi-download
+          </v-icon>
+          Export Settings
+        </v-btn>
+      </v-col>
+      <v-col cols="12" sm="6">
+        <v-btn color="secondary" @click="$refs.importSettings.click()">
+          <v-icon left>
+            mdi-upload
+          </v-icon>
+          Import Settings
+        </v-btn>
+      </v-col>
+
+      <v-col cols="12" sm="6">
+        <v-btn color="secondary" @click="$refs.exportLogFile.click()">
+          <v-icon left>
+            mdi-file
+          </v-icon>
+          Export current log
+
+          <!-- Special hidden link that gets 'clicked' when the user exports journalctl logs -->
+          <a
+            ref="exportLogFile"
+            style="color: black; text-decoration: none; display: none"
+            :href="
+              'http://' +
+                this.$address +
+                '/api/settings/photonvision-journalctl.txt'
+            "
+            download="photonvision-journalctl.txt"
+          />
+        </v-btn>
+      </v-col>
+
+      <v-col cols="12" sm="6">
+        <v-btn color="secondary" @click="showLogs()">
+          <v-icon left>
+            mdi-bug
+          </v-icon>
+          Show log viewer
+        </v-btn>
+      </v-col>
+    </v-row>
+    <v-snackbar v-model="snack" top :color="snackbar.color" timeout="-1">
+      <span>{{ snackbar.text }}</span>
+    </v-snackbar>
+
+    <!-- Special hidden upload input that gets 'clicked' when the user imports settings -->
+    <input
+      ref="importSettings"
+      type="file"
+      accept=".zip, .json"
+      style="display: none;"
+      @change="readImportedSettings"
+    />
+    <!-- Special hidden link that gets 'clicked' when the user exports settings -->
+    <a
+      ref="exportSettings"
+      style="color: black; text-decoration: none; display: none"
+      :href="
+        'http://' + this.$address + '/api/settings/photonvision_config.zip'
+      "
+      download="photonvision-settings.zip"
+    />
+
+    <!-- Special hidden new jar upload input that gets 'clicked' when the user posts a new .jar -->
+    <input
+      ref="offlineUpdate"
+      type="file"
+      accept=".jar"
+      style="display: none;"
+      @change="doOfflineUpdate"
+    />
+  </div>
+</template>
+
+<script>
+export default {
+  name: "Device Control",
+  data() {
+    return {
+      snack: false,
+      uploadPercentage: 0.0,
+      snackbar: {
+        color: "success",
+        text: "",
+      },
+    };
+  },
+  computed: {
+    settings() {
+      return this.$store.state.settings.general;
+    },
+    version() {
+      return `${this.settings.version}`;
+    },
+    hwModel() {
+      if (this.settings.hardwareModel !== "") {
+        return `${this.settings.hardwareModel}`;
+      } else {
+        return `Unknown`;
+      }
+    },
+    platform() {
+      return `${this.settings.hardwarePlatform}`;
+    },
+    gpuAccel() {
+      return `${this.settings.gpuAcceleration ? "Enabled" : "Unsupported"} ${
+        this.settings.gpuAcceleration
+          ? "(" + this.settings.gpuAcceleration + ")"
+          : ""
+      }`;
+    },
+    metrics() {
+      // console.log(this.$store.state.metrics);
+      return this.$store.state.metrics;
+    },
+  },
+  methods: {
+    restartProgram() {
+      this.axios.post("http://" + this.$address + "/api/restartProgram", {});
+    },
+    restartDevice() {
+      this.axios.post("http://" + this.$address + "/api/restartDevice", {});
+    },
+    readImportedSettings(event) {
+      let formData = new FormData();
+      formData.append("zipData", event.target.files[0]);
+      this.axios
+        .post("http://" + this.$address + "/api/settings/import", formData, {
+          headers: { "Content-Type": "multipart/form-data" },
+        })
+        .then(() => {
+          this.snackbar = {
+            color: "success",
+            text:
+              "Settings imported successfully! PhotonVision will restart in the background...",
+          };
+          this.snack = true;
+        })
+        .catch((err) => {
+          if (err.response) {
+            this.snackbar = {
+              color: "error",
+              text:
+                "Error while uploading settings file! Could not process provided file.",
+            };
+          } else if (err.request) {
+            this.snackbar = {
+              color: "error",
+              text:
+                "Error while uploading settings file! No respond to upload attempt.",
+            };
+          } else {
+            this.snackbar = {
+              color: "error",
+              text: "Error while uploading settings file!",
+            };
+          }
+          this.snack = true;
+        });
+    },
+    doOfflineUpdate(event) {
+      this.snackbar = {
+        color: "secondary",
+        text: "New Software Upload in Process...",
+      };
+      this.snack = true;
+
+      let formData = new FormData();
+      formData.append("jarData", event.target.files[0]);
+      this.axios
+        .post(
+          "http://" + this.$address + "/api/settings/offlineUpdate",
+          formData,
+          {
+            headers: { "Content-Type": "multipart/form-data" },
+            onUploadProgress: function(progressEvent) {
+              this.uploadPercentage = parseInt(
+                Math.round((progressEvent.loaded / progressEvent.total) * 100)
+              );
+              if (this.uploadPercentage < 99.5) {
+                this.snackbar.text =
+                  "New Software Upload in Process, " +
+                  this.uploadPercentage +
+                  "% complete";
+              } else {
+                this.snackbar.text = "Installing uploaded software...";
+              }
+            }.bind(this),
+          }
+        )
+        .then(() => {
+          this.snackbar = {
+            color: "success",
+            text:
+              "New .jar copied successfully! PhotonVision will restart in the background...",
+          };
+          this.snack = true;
+        })
+        .catch((err) => {
+          if (err.response) {
+            this.snackbar = {
+              color: "error",
+              text:
+                "Error while uploading new .jar file! Could not process provided file.",
+            };
+          } else if (err.request) {
+            this.snackbar = {
+              color: "error",
+              text:
+                "Error while uploading new .jar file! No respond to upload attempt.",
+            };
+          } else {
+            this.snackbar = {
+              color: "error",
+              text: "Error while uploading new .jar file!",
+            };
+          }
+          this.snack = true;
+        });
+    },
+    showLogs(event) {
+      event;
+      this.$store.state.logsOverlay = true;
+    },
+  },
+};
+</script>
+
+<style lang="css" scoped>
+.v-btn {
+  width: 100%;
+}
+
+.infoTable {
+  border: 1px solid;
+  border-collapse: separate;
+  border-spacing: 0px;
+  border-radius: 5px;
+  text-align: left;
+  margin-bottom: 10px;
+  width: 100%;
+  display: block;
+  overflow-x: auto;
+}
+
+.infoElem {
+  padding-right: 15px;
+  padding-bottom: 1px;
+  padding-top: 1px;
+  padding-left: 10px;
+  border-right: 1px solid;
+}
+</style>

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-client/src/views/SettingsViews/Stats.vue

@@ -117,85 +117,11 @@
       </table>
     </v-row>
 
-    <v-row>
-      <v-col
-        cols="12"
-        sm="6"
-        md="4"
-      >
-        <v-btn
-          color="secondary"
-          @click="$refs.exportSettings.click()"
-        >
-          <v-icon left>
-            mdi-download
-          </v-icon>
-          Export Settings
-        </v-btn>
-      </v-col>
-      <v-col
-        cols="12"
-        sm="6"
-        md="4"
-      >
-        <v-btn
-          color="secondary"
-          @click="$refs.importSettings.click()"
-        >
-          <v-icon left>
-            mdi-upload
-          </v-icon>
-          Import Settings
-        </v-btn>
-      </v-col>
-      <v-col
-        cols="12"
-        md="4"
-      >
-        <v-btn
-          color="secondary"
-          @click="$refs.offlineUpdate.click()"
-        >
-          <v-icon left>
-            mdi-update
-          </v-icon>
-          Offline Update
-        </v-btn>
-      </v-col>
-      <v-col
-        cols="12"
-        lg="6"
-      >
-        <v-btn
-          color="red"
-          @click="restartProgram()"
-        >
-          <v-icon left>
-            mdi-restart
-          </v-icon>
-          Restart PhotonVision
-        </v-btn>
-      </v-col>
-      <v-col
-        cols="12"
-        lg="6"
-      >
-        <v-btn
-          color="red"
-          @click="restartDevice()"
-        >
-          <v-icon left>
-            mdi-restart-alert
-          </v-icon>
-          Restart Device
-        </v-btn>
-      </v-col>
-    </v-row>
     <v-snackbar
       v-model="snack"
       top
       :color="snackbar.color"
-      timeout="0"
+      timeout="-1"
     >
       <span>{{ snackbar.text }}</span>
     </v-snackbar>
@@ -230,7 +156,7 @@
 
 <script>
 export default {
-    name: 'General',
+    name: 'Stats',
     data() {
         return {
             snack: false,
@@ -262,8 +188,8 @@ export default {
           return  `${this.settings.gpuAcceleration ? "Enabled" : "Unsupported"} ${this.settings.gpuAcceleration ? "(" + this.settings.gpuAcceleration + ")" : ""}`
         },
         metrics() {
-          console.log(this.$store.state.metrics);
-            return this.$store.state.metrics;
+          // console.log(this.$store.state.metrics);
+          return this.$store.state.metrics;
         }
     },
     methods: {
@@ -349,6 +275,10 @@ export default {
                 this.snack = true;
             });
         },
+        showLogs(event) {
+          event;
+          this.$store.state.logsOverlay = true;
+        }
     }
 }
 </script>

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(
@@ -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?
@@ -189,11 +198,21 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
     public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
         var ret = new ArrayList<PhotonTrackedTarget>();
         for (var t : targets) {
-            var points = new Point[4];
-            t.getMinAreaRect().points(points);
-            var cornerList = new ArrayList<TargetCorner>();
-
-            for (int i = 0; i < 4; i++) cornerList.add(new TargetCorner(points[i].x, points[i].y));
+            var minAreaRectCorners = new ArrayList<TargetCorner>();
+            var detectedCorners = new ArrayList<TargetCorner>();
+            {
+                var points = new Point[4];
+                t.getMinAreaRect().points(points);
+                for (int i = 0; i < 4; i++) {
+                    minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y));
+                }
+            }
+            {
+                var points = t.getTargetCorners();
+                for (int i = 0; i < points.size(); i++) {
+                    detectedCorners.add(new TargetCorner(points.get(i).x, points.get(i).y));
+                }
+            }
 
             ret.add(
                     new PhotonTrackedTarget(
@@ -205,7 +224,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
                             t.getBestCameraToTarget3d(),
                             t.getAltCameraToTarget3d(),
                             t.getPoseAmbiguity(),
-                            cornerList));
+                            minAreaRectCorners,
+                            detectedCorners));
         }
         return ret;
     }

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));
@@ -196,14 +225,18 @@ public class TestUtils {
 
     public enum ApriltagTestImages {
         kRobots,
-        kTag1_640_480;
+        kTag1_640_480,
+        kTag1_16h5_1280,
+        kTag_corner_1280;
 
         public final Path path;
 
         Path getPath() {
             // Strip leading k
             var filename = this.toString().substring(1).toLowerCase();
-            return Path.of("apriltag", filename + ".jpg");
+            var extension = ".jpg";
+            if (filename.equals("tag1_16h5_1280")) extension = ".png";
+            return Path.of("apriltag", filename + extension);
         }
 
         ApriltagTestImages() {
@@ -211,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
@@ -302,6 +335,8 @@ 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) {
         try {
@@ -350,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/common/util/math/MathUtils.java

@@ -172,6 +172,17 @@ public class MathUtils {
     private static final Rotation3d WPILIB_BASE_ROTATION =
             new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 1, 0, 0, 0, 1, 1, 0, 0));
 
+    public static Transform3d convertOpenCVtoPhotonTransform(Transform3d cameraToTarget3d) {
+        // TODO: Refactor into new pipe?
+        // CameraToTarget _should_ be in opencv-land EDN
+        var nwu =
+                CoordinateSystem.convert(
+                        new Pose3d().transformBy(cameraToTarget3d),
+                        CoordinateSystem.EDN(),
+                        CoordinateSystem.NWU());
+        return new Transform3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
+    }
+
     public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
         // TODO: Refactor into new pipe?
         // CameraToTarget _should_ be in opencv-land EDN
@@ -208,6 +219,14 @@ public class MathUtils {
         return new Transform3d(pose.getTranslation(), ocvRotation);
     }
 
+    public static Pose3d convertArucotoOpenCV(Transform3d pose) {
+        var ocvRotation =
+                APRILTAG_BASE_ROTATION.rotateBy(
+                        new Rotation3d(VecBuilder.fill(0, 0, 1), Units.degreesToRadians(180))
+                                .rotateBy(pose.getRotation()));
+        return new Pose3d(pose.getTranslation(), ocvRotation);
+    }
+
     public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) {
         var angle = rotation.getAngle();
         var axis = rotation.getAxis().times(angle);

photon-core/src/main/java/org/photonvision/vision/aruco/ArucoDetectionResult.java

@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.aruco;
+
+import java.util.Arrays;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class ArucoDetectionResult {
+    private static final Logger logger =
+            new Logger(ArucoDetectionResult.class, LogGroup.VisionModule);
+    double[] xCorners;
+    double[] yCorners;
+
+    int id;
+
+    double[] tvec, rvec;
+
+    public ArucoDetectionResult(
+            double[] xCorners, double[] yCorners, int id, double[] tvec, double[] rvec) {
+        this.xCorners = xCorners;
+        this.yCorners = yCorners;
+        this.id = id;
+        this.tvec = tvec;
+        this.rvec = rvec;
+        // logger.debug("Creating a new detection result: " + this.toString());
+    }
+
+    public double[] getTvec() {
+        return tvec;
+    }
+
+    public double[] getRvec() {
+        return rvec;
+    }
+
+    public double[] getxCorners() {
+        return xCorners;
+    }
+
+    public double[] getyCorners() {
+        return yCorners;
+    }
+
+    public int getId() {
+        return id;
+    }
+
+    public double getCenterX() {
+        return (xCorners[0] + xCorners[1] + xCorners[2] + xCorners[3]) * .25;
+    }
+
+    public double getCenterY() {
+        return (yCorners[0] + yCorners[1] + yCorners[2] + yCorners[3]) * .25;
+    }
+
+    @Override
+    public String toString() {
+        return "ArucoDetectionResult{"
+                + "xCorners="
+                + Arrays.toString(xCorners)
+                + ", yCorners="
+                + Arrays.toString(yCorners)
+                + ", id="
+                + id
+                + '}';
+    }
+}

photon-core/src/main/java/org/photonvision/vision/aruco/ArucoDetectorParams.java

@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.aruco;
+
+import org.opencv.aruco.Aruco;
+import org.opencv.aruco.ArucoDetector;
+import org.opencv.aruco.DetectorParameters;
+import org.opencv.aruco.Dictionary;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class ArucoDetectorParams {
+    private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
+
+    private float m_decimate = -1;
+    private int m_iterations = -1;
+    private double m_accuracy = -1;
+
+    DetectorParameters parameters = DetectorParameters.create();
+    ArucoDetector detector;
+
+    public ArucoDetectorParams() {
+        setDecimation(1);
+        setCornerAccuracy(25);
+        setCornerRefinementMaxIterations(100);
+
+        detector = new ArucoDetector(Dictionary.get(Aruco.DICT_APRILTAG_16h5), parameters);
+    }
+
+    public void setDecimation(float decimate) {
+        if (decimate == m_decimate) return;
+
+        logger.info("Setting decimation from " + m_decimate + " to " + decimate);
+
+        // We only need to mutate the parameters -- the detector keeps a poitner to the parameters
+        // object internally, so it should automatically update
+        parameters.set_aprilTagQuadDecimate((float) decimate);
+        m_decimate = decimate;
+    }
+
+    public void setCornerRefinementMaxIterations(int iters) {
+        if (iters == m_iterations || iters <= 0) return;
+
+        parameters.set_cornerRefinementMethod(Aruco.CORNER_REFINE_SUBPIX);
+        parameters.set_cornerRefinementMaxIterations(iters); // 200
+
+        m_iterations = iters;
+    }
+
+    public void setCornerAccuracy(double accuracy) {
+        if (accuracy == m_accuracy || accuracy <= 0) return;
+
+        parameters.set_cornerRefinementMinAccuracy(
+                accuracy / 1000.0); // divides by 1000 because the UI multiplies it by 1000
+        m_accuracy = accuracy;
+    }
+
+    public ArucoDetector getDetector() {
+        return detector;
+    }
+}

photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java

@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.aruco;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import java.util.ArrayList;
+import org.opencv.aruco.Aruco;
+import org.opencv.aruco.ArucoDetector;
+import org.opencv.core.Mat;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+
+public class PhotonArucoDetector {
+    private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
+
+    private static final Rotation3d ARUCO_BASE_ROTATION =
+            new Rotation3d(VecBuilder.fill(0, 0, 1), Units.degreesToRadians(180));
+
+    Mat ids;
+
+    Mat tvecs;
+    Mat rvecs;
+    ArrayList<Mat> corners;
+
+    Mat cornerMat;
+    Translation3d translation;
+    Rotation3d rotation;
+    double timeStartDetect;
+    double timeEndDetect;
+    Pose3d tagPose;
+    double timeStartProcess;
+    double timeEndProcess;
+    double[] xCorners = new double[4];
+    double[] yCorners = new double[4];
+
+    public PhotonArucoDetector() {
+        logger.debug("New Aruco Detector");
+        ids = new Mat();
+        tvecs = new Mat();
+        rvecs = new Mat();
+        corners = new ArrayList<Mat>();
+        tagPose = new Pose3d();
+        translation = new Translation3d();
+        rotation = new Rotation3d();
+    }
+
+    public ArucoDetectionResult[] detect(
+            Mat grayscaleImg,
+            float tagSize,
+            CameraCalibrationCoefficients coeffs,
+            ArucoDetector detector) {
+        detector.detectMarkers(grayscaleImg, corners, ids);
+        if (coeffs != null) {
+            Aruco.estimatePoseSingleMarkers(
+                    corners,
+                    tagSize,
+                    coeffs.getCameraIntrinsicsMat(),
+                    coeffs.getDistCoeffsMat(),
+                    rvecs,
+                    tvecs);
+        }
+
+        ArucoDetectionResult[] toReturn = new ArucoDetectionResult[corners.size()];
+        timeStartProcess = System.currentTimeMillis();
+        for (int i = 0; i < corners.size(); i++) {
+            cornerMat = corners.get(i);
+            // logger.debug(cornerMat.dump());
+            xCorners =
+                    new double[] {
+                        cornerMat.get(0, 0)[0],
+                        cornerMat.get(0, 1)[0],
+                        cornerMat.get(0, 2)[0],
+                        cornerMat.get(0, 3)[0]
+                    };
+            yCorners =
+                    new double[] {
+                        cornerMat.get(0, 0)[1],
+                        cornerMat.get(0, 1)[1],
+                        cornerMat.get(0, 2)[1],
+                        cornerMat.get(0, 3)[1]
+                    };
+            cornerMat.release();
+
+            double[] tvec;
+            double[] rvec;
+            if (coeffs != null) {
+                // Need to apply a 180 rotation about Z
+                var origRvec = rvecs.get(i, 0);
+                var axisangle = VecBuilder.fill(origRvec[0], origRvec[1], origRvec[2]);
+                Rotation3d rotation = new Rotation3d(axisangle, axisangle.normF());
+                var ocvRotation = ARUCO_BASE_ROTATION.rotateBy(rotation);
+
+                var angle = ocvRotation.getAngle();
+                var finalAxisAngle = ocvRotation.getAxis().times(angle);
+
+                tvec = tvecs.get(i, 0);
+                rvec = finalAxisAngle.getData();
+            } else {
+                tvec = new double[] {0, 0, 0};
+                rvec = new double[] {0, 0, 0};
+            }
+
+            toReturn[i] =
+                    new ArucoDetectionResult(xCorners, yCorners, (int) ids.get(i, 0)[0], tvec, rvec);
+        }
+        rvecs.release();
+        tvecs.release();
+        ids.release();
+
+        return toReturn;
+    }
+}

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/camera/LibcameraGpuSettables.java

@@ -29,13 +29,15 @@ import org.photonvision.vision.processes.VisionSourceSettables;
 
 public class LibcameraGpuSettables extends VisionSourceSettables {
     private FPSRatedVideoMode currentVideoMode;
-    private double lastExposure = 50;
+    private double lastManualExposure = 50;
     private int lastBrightness = 50;
-    private boolean lastExposureMode;
+    private boolean lastAutoExposureActive;
     private int lastGain = 50;
     private Pair<Integer, Integer> lastAwbGains = new Pair<>(18, 18);
     private boolean m_initialized = false;
 
+    private final LibCameraJNI.SensorModel sensorModel;
+
     private ImageRotationMode m_rotationMode;
 
     public void setRotation(ImageRotationMode rotationMode) {
@@ -51,7 +53,7 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
 
         videoModes = new HashMap<>();
 
-        LibCameraJNI.SensorModel sensorModel = LibCameraJNI.getSensorModel();
+        sensorModel = LibCameraJNI.getSensorModel();
 
         if (sensorModel == LibCameraJNI.SensorModel.IMX219) {
             // Settings for the IMX219 sensor, which is used on the Pi Camera Module v2
@@ -72,13 +74,14 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
                     6, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 3280 / 4, 2464 / 4, 15, 20, 1));
         } else if (sensorModel == LibCameraJNI.SensorModel.OV9281) {
             videoModes.put(
-                    0, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1280, 800, 60, 60, 1));
+                    0, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 320, 240, 30, 30, .39));
             videoModes.put(
                     1, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1280 / 2, 800 / 2, 60, 60, 1));
             videoModes.put(
-                    2, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 320, 240, 30, 30, .39));
+                    2, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 640, 480, 65, 90, .39));
             videoModes.put(
-                    3, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 640, 480, 65, 90, .39));
+                    3, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1280, 800, 60, 60, 1));
+
         } else {
             if (sensorModel == LibCameraJNI.SensorModel.IMX477) {
                 LibcameraGpuSource.logger.warn(
@@ -114,20 +117,33 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
 
     @Override
     public void setAutoExposure(boolean cameraAutoExposure) {
-        lastExposureMode = cameraAutoExposure;
-        // TODO (Matt) -- call LibCameraJNI's auto exposure function, when that exists
+        lastAutoExposureActive = cameraAutoExposure;
         LibCameraJNI.setAutoExposure(cameraAutoExposure);
     }
 
     @Override
     public void setExposure(double exposure) {
-        // Todo (Chris) - for now, handle auto exposure by using -1
-        if (exposure < 0.0) {
-            exposure = -1;
+        if (exposure < 0.0 || lastAutoExposureActive) {
+            // Auto-exposure is active right now, don't set anything.
+            return;
         }
 
-        // TODO convert to uS
-        lastExposure = exposure;
+        // HACKS!
+        // If we set exposure too low, libcamera crashes or slows down
+        // Very weird and smelly
+        // For now, band-aid this by just not setting it lower than the "it breaks" limit
+        // Limit is different depending on camera.
+        if (sensorModel == LibCameraJNI.SensorModel.OV9281) {
+            if (exposure < 6.0) {
+                exposure = 6.0;
+            }
+        } else if (sensorModel == LibCameraJNI.SensorModel.OV5647) {
+            if (exposure < 0.7) {
+                exposure = 0.7;
+            }
+        }
+
+        lastManualExposure = exposure;
         var success = LibCameraJNI.setExposure((int) Math.round(exposure) * 800);
         if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera exposure");
     }
@@ -150,19 +166,25 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
 
     @Override
     public void setRedGain(int red) {
-        lastAwbGains = Pair.of(red, lastAwbGains.getSecond());
-        setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
+        if (sensorModel != LibCameraJNI.SensorModel.OV9281) {
+            lastAwbGains = Pair.of(red, lastAwbGains.getSecond());
+            setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
+        }
     }
 
     @Override
     public void setBlueGain(int blue) {
-        lastAwbGains = Pair.of(lastAwbGains.getFirst(), blue);
-        setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
+        if (sensorModel != LibCameraJNI.SensorModel.OV9281) {
+            lastAwbGains = Pair.of(lastAwbGains.getFirst(), blue);
+            setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
+        }
     }
 
     public void setAwbGain(int red, int blue) {
-        var success = LibCameraJNI.setAwbGain(red / 10.0, blue / 10.0);
-        if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera AWB gains");
+        if (sensorModel != LibCameraJNI.SensorModel.OV9281) {
+            var success = LibCameraJNI.setAwbGain(red / 10.0, blue / 10.0);
+            if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera AWB gains");
+        }
     }
 
     @Override
@@ -202,8 +224,8 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
 
         // We don't store last settings on the native side, and when you change video mode these get
         // reset on MMAL's end
-        setExposure(lastExposure);
-        setAutoExposure(lastExposureMode);
+        setExposure(lastManualExposure);
+        setAutoExposure(lastAutoExposureActive);
         setBrightness(lastBrightness);
         setGain(lastGain);
         setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());

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/frame/consumer/FileSaveFrameConsumer.java

@@ -17,20 +17,18 @@
 
 package org.photonvision.vision.frame.consumer;
 
-import edu.wpi.first.networktables.BooleanEntry;
+import edu.wpi.first.networktables.IntegerEntry;
 import edu.wpi.first.networktables.NetworkTable;
 import java.io.File;
 import java.text.DateFormat;
 import java.text.SimpleDateFormat;
 import java.util.Date;
-import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.Consumer;
 import org.opencv.imgcodecs.Imgcodecs;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
-import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.vision.opencv.CVMat;
 
 public class FileSaveFrameConsumer implements Consumer<CVMat> {
@@ -44,29 +42,27 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
     private NetworkTable subTable;
     private final NetworkTable rootTable;
     private final Logger logger;
-    private boolean prevCommand = false;
+    private long imgSaveCountInternal = 0;
     private String camNickname;
     private String fnamePrefix;
-    private final long CMD_RESET_TIME_MS = 500;
-    private final BooleanEntry entry;
-    // Helps prevent race conditions between user set & auto-reset logic
-    private ReentrantLock lock;
+    private IntegerEntry entry;
 
     public FileSaveFrameConsumer(String camNickname, String streamPrefix) {
-        this.lock = new ReentrantLock();
         this.fnamePrefix = camNickname + "_" + streamPrefix;
         this.ntEntryName = streamPrefix + NT_SUFFIX;
         this.rootTable = NetworkTablesManager.getInstance().kRootTable;
         updateCameraNickname(camNickname);
-        entry = subTable.getBooleanTopic(ntEntryName).getEntry(false);
         this.logger = new Logger(FileSaveFrameConsumer.class, this.camNickname, LogGroup.General);
     }
 
     public void accept(CVMat image) {
         if (image != null && image.getMat() != null && !image.getMat().empty()) {
-            if (lock.tryLock()) {
-                boolean curCommand = entry.get(false);
-                if (curCommand && !prevCommand) {
+            var curCommand = entry.get(); // default to just our current count
+            if (curCommand >= 0) {
+                // Only do something if we got a valid current command
+                if (imgSaveCountInternal < curCommand) {
+                    // Save one frame.
+                    // Create the filename
                     Date now = new Date();
                     String savefile =
                             FILE_PATH
@@ -78,42 +74,32 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
                                     + tf.format(now)
                                     + FILE_EXTENSION;
 
+                    // write to file
                     Imgcodecs.imwrite(savefile, image.getMat());
 
-                    // Help the user a bit - set the NT entry back to false after 500ms
-                    TimedTaskManager.getInstance().addOneShotTask(this::resetCommand, CMD_RESET_TIME_MS);
-
+                    // Count one more image saved
+                    imgSaveCountInternal++;
                     logger.info("Saved new image at " + savefile);
-                } else if (!curCommand) {
-                    // If the entry is currently false, set it again. This will make sure it shows up on the
-                    // dashboard.
-                    entry.set(false);
+
+                } else if (imgSaveCountInternal > curCommand) {
+                    imgSaveCountInternal = curCommand;
                 }
-
-                prevCommand = curCommand;
-                lock.unlock();
-            }
-        }
-    }
-
-    private void resetCommand() {
-        lock.lock();
-        this.subTable.getEntry(ntEntryName).setBoolean(false);
-        lock.unlock();
-    }
-
-    private void removeEntries() {
-        if (this.subTable != null) {
-            if (this.subTable.containsKey(ntEntryName)) {
-                this.subTable.getEntry(ntEntryName).close();
             }
         }
     }
 
     public void updateCameraNickname(String newCameraNickname) {
-        removeEntries();
+        // Remove existing entries
+        if (this.subTable != null) {
+            if (this.subTable.containsKey(ntEntryName)) {
+                this.subTable.getEntry(ntEntryName).close();
+            }
+        }
+
+        // Recreate and re-init network tables structure
         this.camNickname = newCameraNickname;
         this.subTable = rootTable.getSubTable(this.camNickname);
-        resetCommand();
+        this.subTable.getEntry(ntEntryName).setInteger(imgSaveCountInternal);
+        this.entry = subTable.getIntegerTopic(ntEntryName).getEntry(-1); // Default negative
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java

@@ -21,6 +21,10 @@ import edu.wpi.first.apriltag.AprilTagDetection;
 import edu.wpi.first.apriltag.AprilTagPoseEstimate;
 import edu.wpi.first.apriltag.AprilTagPoseEstimator;
 import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipe.CVPipe;
 
 public class AprilTagPoseEstimatorPipe
@@ -37,9 +41,48 @@ public class AprilTagPoseEstimatorPipe
         super();
     }
 
+    MatOfPoint2f temp = new MatOfPoint2f();
+
     @Override
     protected AprilTagPoseEstimate process(AprilTagDetection in) {
-        return m_poseEstimator.estimateOrthogonalIteration(in, params.nIters);
+        // Save the corner points of our detection to an array
+        Point corners[] = new Point[4];
+        for (int i = 0; i < 4; i++) {
+            corners[i] = new Point(in.getCornerX(i), in.getCornerY(i));
+        }
+        // And shove into our matofpoints
+        temp.fromArray(corners);
+
+        // Probably overwrites what was in temp before. I hope
+        Calib3d.undistortImagePoints(
+                temp,
+                temp,
+                params.calibration.getCameraIntrinsicsMat(),
+                params.calibration.getDistCoeffsMat());
+
+        // Save out undistorted corners
+        corners = temp.toArray();
+
+        // Apriltagdetection expects an array in form [x1 y1 x2 y2 ...]
+        var fixedCorners = new double[8];
+        for (int i = 0; i < 4; i++) {
+            fixedCorners[i * 2] = corners[i].x;
+            fixedCorners[i * 2 + 1] = corners[i].y;
+        }
+
+        // Create a new Detection with the fixed corners
+        var corrected =
+                new AprilTagDetection(
+                        in.getFamily(),
+                        in.getId(),
+                        in.getHamming(),
+                        in.getDecisionMargin(),
+                        in.getHomography(),
+                        in.getCenterX(),
+                        in.getCenterY(),
+                        fixedCorners);
+
+        return m_poseEstimator.estimateOrthogonalIteration(corrected, params.nIters);
     }
 
     @Override
@@ -57,11 +100,14 @@ public class AprilTagPoseEstimatorPipe
 
     public static class AprilTagPoseEstimatorPipeParams {
         final AprilTagPoseEstimator.Config config;
+        final CameraCalibrationCoefficients calibration;
         final int nIters;
 
-        public AprilTagPoseEstimatorPipeParams(Config config, int nIters) {
+        public AprilTagPoseEstimatorPipeParams(
+                Config config, CameraCalibrationCoefficients cal, int nIters) {
             this.config = config;
             this.nIters = nIters;
+            this.calibration = cal;
         }
 
         @Override

photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java

@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.opencv.aruco.DetectorParameters;
+import org.opencv.core.Mat;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
+import org.photonvision.vision.aruco.PhotonArucoDetector;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class ArucoDetectionPipe
+        extends CVPipe<Mat, List<ArucoDetectionResult>, ArucoDetectionPipeParams> {
+    PhotonArucoDetector detector = new PhotonArucoDetector();
+
+    @Override
+    protected List<ArucoDetectionResult> process(Mat in) {
+        return List.of(
+                detector.detect(
+                        in,
+                        (float) Units.inchesToMeters(6),
+                        params.cameraCalibrationCoefficients,
+                        params.detectorParams));
+    }
+
+    @Override
+    public void setParams(ArucoDetectionPipeParams params) {
+        super.setParams(params);
+    }
+
+    public DetectorParameters getParameters() {
+        return params == null ? null : params.detectorParams.get_params();
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipeParams.java

@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import java.util.Objects;
+import org.opencv.aruco.ArucoDetector;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+
+public class ArucoDetectionPipeParams {
+    public ArucoDetector detectorParams;
+    public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
+
+    public ArucoDetectionPipeParams(
+            ArucoDetector detector, CameraCalibrationCoefficients cameraCalibrationCoefficients) {
+        this.detectorParams = detector;
+        this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        ArucoDetectionPipeParams that = (ArucoDetectionPipeParams) o;
+        return Objects.equals(detectorParams, that.detectorParams)
+                && Objects.equals(cameraCalibrationCoefficients, that.cameraCalibrationCoefficients);
+    }
+
+    @Override
+    public int hashCode() {
+        return Objects.hash(detectorParams, cameraCalibrationCoefficients);
+    }
+
+    @Override
+    public String toString() {
+        return "ArucoDetectionPipeParams{"
+                + "detectorParams="
+                + detectorParams
+                + ", cameraCalibrationCoefficients="
+                + cameraCalibrationCoefficients
+                + '}';
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java

@@ -47,7 +47,7 @@ public class CornerDetectionPipe
                     {
                         var targetCorners =
                                 detectExtremeCornersByApproxPolyDp(target, params.calculateConvexHulls);
-                        target.setCorners(targetCorners);
+                        target.setTargetCorners(targetCorners);
                         break;
                     }
                 default:

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dArucoPipe.java

@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import java.awt.*;
+import org.photonvision.vision.frame.FrameDivisor;
+
+public class Draw2dArucoPipe extends Draw2dTargetsPipe {
+    public static class Draw2dArucoParams extends Draw2dTargetsPipe.Draw2dTargetsParams {
+        public Draw2dArucoParams(
+                boolean shouldDraw, boolean showMultipleTargets, FrameDivisor divisor) {
+            super(shouldDraw, showMultipleTargets, divisor);
+            // We want to show the polygon, not the rotated box
+            this.showRotatedBox = false;
+            this.showMaximumBox = false;
+            this.rotatedBoxColor = Color.RED;
+        }
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dAprilTagsPipe.java

@@ -30,6 +30,7 @@ public class Draw3dAprilTagsPipe extends Draw3dTargetsPipe {
                 FrameDivisor divisor) {
             super(shouldDraw, cameraCalibrationCoefficients, targetModel, divisor);
             this.shouldDrawHull = false;
+            this.redistortPoints = true;
         }
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dArucoPipe.java

@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+import org.photonvision.vision.frame.FrameDivisor;
+import org.photonvision.vision.target.TargetModel;
+
+public class Draw3dArucoPipe extends Draw3dTargetsPipe {
+    public static class Draw3dArucoParams extends Draw3dContoursParams {
+        public Draw3dArucoParams(
+                boolean shouldDraw,
+                CameraCalibrationCoefficients cameraCalibrationCoefficients,
+                TargetModel targetModel,
+                FrameDivisor divisor) {
+            super(shouldDraw, cameraCalibrationCoefficients, targetModel, divisor);
+            this.shouldDrawHull = false;
+        }
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java

@@ -93,7 +93,12 @@ public class Draw3dTargetsPipe
                         params.cameraCalibrationCoefficients.getDistCoeffsMat(),
                         tempMat,
                         jac);
-                // Distort the points so they match the image they're being overlaid on
+
+                if (params.redistortPoints) {
+                    // Distort the points so they match the image they're being overlaid on
+                    distortPoints(tempMat, tempMat);
+                }
+
                 var bottomPoints = tempMat.toList();
 
                 Calib3d.projectPoints(
@@ -104,6 +109,11 @@ public class Draw3dTargetsPipe
                         params.cameraCalibrationCoefficients.getDistCoeffsMat(),
                         tempMat,
                         jac);
+
+                if (params.redistortPoints) {
+                    // Distort the points so they match the image they're being overlaid on
+                    distortPoints(tempMat, tempMat);
+                }
                 var topPoints = tempMat.toList();
 
                 dividePointList(bottomPoints);
@@ -290,6 +300,8 @@ public class Draw3dTargetsPipe
         public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
         public final FrameDivisor divisor;
 
+        public boolean redistortPoints = false;
+
         public Draw3dContoursParams(
                 boolean shouldDraw,
                 CameraCalibrationCoefficients cameraCalibrationCoefficients,

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/pipe/impl/FindCirclesPipe.java

@@ -66,7 +66,7 @@ public class FindCirclesPipe
                 1.0,
                 params.minDist,
                 params.maxCannyThresh,
-                params.accuracy,
+                Math.max(1.0, params.accuracy),
                 minRadius,
                 maxRadius);
         // Great, we now found the center point of the circle and it's radius, but we have no idea what

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java

@@ -69,28 +69,7 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
         // target model for the draw 3d targets pipeline to work...
 
         // for now, hard code tag width based on enum value
-        double tagWidth;
-
-        // This needs
-        switch (settings.targetModel) {
-            case k200mmAprilTag:
-                {
-                    tagWidth = Units.inchesToMeters(3.25 * 2);
-                    break;
-                }
-            case k6in_16h5:
-                {
-                    tagWidth = Units.inchesToMeters(3 * 2);
-                    break;
-                }
-            default:
-                {
-                    // guess at 200mm?? If it's zero everything breaks, but it should _never_ be zero. Unless
-                    // users select the wrong model...
-                    tagWidth = 0.16;
-                    break;
-                }
-        }
+        double tagWidth = Units.inchesToMeters(3 * 2); // for 6in 16h5 tag.
 
         // AprilTagDetectorParams aprilTagDetectionParams =
         //         new AprilTagDetectorParams(
@@ -118,7 +97,9 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
 
                 poseEstimatorPipe.setParams(
                         new AprilTagPoseEstimatorPipeParams(
-                                new Config(tagWidth, fx, fy, cx, cy), settings.numIterations));
+                                new Config(tagWidth, fx, fy, cx, cy),
+                                frameStaticProperties.cameraCalibration,
+                                settings.numIterations));
             }
         }
     }

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java

@@ -24,17 +24,15 @@ import org.photonvision.vision.target.TargetModel;
 
 @JsonTypeName("AprilTagPipelineSettings")
 public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
-    public AprilTagFamily tagFamily = AprilTagFamily.kTag36h11;
+    public AprilTagFamily tagFamily = AprilTagFamily.kTag16h5;
     public int decimate = 1;
     public double blur = 0;
-    public int threads = 1;
+    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;
-
-    // TODO is this a legit, reasonable default?
-    public int hammingDist = 1;
-    public int decisionMargin = 30;
+    public int numIterations = 40;
+    public int hammingDist = 0;
+    public int decisionMargin = 35;
 
     // 3d settings
 
@@ -42,7 +40,9 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
         super();
         pipelineType = PipelineType.AprilTag;
         outputShowMultipleTargets = true;
-        targetModel = TargetModel.k200mmAprilTag;
+        targetModel = TargetModel.k6in_16h5;
+        cameraExposure = 20;
+        cameraAutoExposure = false;
         ledMode = false;
     }
 

photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java

@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import java.util.ArrayList;
+import java.util.List;
+import org.opencv.core.Mat;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
+import org.photonvision.vision.aruco.ArucoDetectorParams;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.frame.FrameThresholdType;
+import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
+import org.photonvision.vision.pipe.impl.*;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TrackedTarget;
+import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
+
+@SuppressWarnings("DuplicatedCode")
+public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSettings> {
+    private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
+    private final GrayscalePipe grayscalePipe = new GrayscalePipe();
+
+    private final ArucoDetectionPipe arucoDetectionPipe = new ArucoDetectionPipe();
+    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
+
+    ArucoDetectorParams m_arucoDetectorParams = new ArucoDetectorParams();
+
+    public ArucoPipeline() {
+        super(FrameThresholdType.GREYSCALE);
+        settings = new ArucoPipelineSettings();
+    }
+
+    public ArucoPipeline(ArucoPipelineSettings settings) {
+        super(FrameThresholdType.GREYSCALE);
+        this.settings = settings;
+    }
+
+    @Override
+    protected void setPipeParamsImpl() {
+        // Sanitize thread count - not supported to have fewer than 1 threads
+        settings.threads = Math.max(1, settings.threads);
+
+        RotateImagePipe.RotateImageParams rotateImageParams =
+                new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
+        rotateImagePipe.setParams(rotateImageParams);
+
+        m_arucoDetectorParams.setDecimation((float) settings.decimate);
+        m_arucoDetectorParams.setCornerRefinementMaxIterations(settings.numIterations);
+        m_arucoDetectorParams.setCornerAccuracy(settings.cornerAccuracy);
+
+        arucoDetectionPipe.setParams(
+                new ArucoDetectionPipeParams(
+                        m_arucoDetectorParams.getDetector(), frameStaticProperties.cameraCalibration));
+    }
+
+    @Override
+    protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) {
+        long sumPipeNanosElapsed = 0L;
+        Mat rawInputMat;
+        rawInputMat = frame.colorImage.getMat();
+
+        List<TrackedTarget> targetList;
+        CVPipeResult<List<ArucoDetectionResult>> tagDetectionPipeResult;
+
+        if (rawInputMat.empty()) {
+            return new CVPipelineResult(sumPipeNanosElapsed, 0, List.of(), frame);
+        }
+
+        tagDetectionPipeResult = arucoDetectionPipe.run(rawInputMat);
+        targetList = new ArrayList<>();
+        for (ArucoDetectionResult detection : tagDetectionPipeResult.output) {
+            // TODO this should be in a pipe, not in the top level here (Matt)
+
+            // populate the target list
+            // Challenge here is that TrackedTarget functions with OpenCV Contour
+            TrackedTarget target =
+                    new TrackedTarget(
+                            detection,
+                            new TargetCalculationParameters(
+                                    false, null, null, null, null, frameStaticProperties));
+
+            var correctedBestPose = target.getBestCameraToTarget3d();
+
+            target.setBestCameraToTarget3d(
+                    new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
+
+            targetList.add(target);
+        }
+
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
+        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, frame);
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java

@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import com.fasterxml.jackson.annotation.JsonTypeName;
+import org.photonvision.vision.target.TargetModel;
+
+@JsonTypeName("ArucoPipelineSettings")
+public class ArucoPipelineSettings extends AdvancedPipelineSettings {
+    public double decimate = 1;
+    public int threads = 2;
+    public int numIterations = 100;
+    public double cornerAccuracy = 25.0;
+    public boolean useAruco3 = true;
+
+    // 3d settings
+
+    public ArucoPipelineSettings() {
+        super();
+        pipelineType = PipelineType.Aruco;
+        outputShowMultipleTargets = true;
+        targetModel = TargetModel.kAruco6in_16h5;
+        cameraExposure = -1;
+        cameraAutoExposure = true;
+        ledMode = false;
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java

@@ -31,7 +31,8 @@ import org.photonvision.vision.opencv.ImageRotationMode;
     @JsonSubTypes.Type(value = ColoredShapePipelineSettings.class),
     @JsonSubTypes.Type(value = ReflectivePipelineSettings.class),
     @JsonSubTypes.Type(value = DriverModePipelineSettings.class),
-    @JsonSubTypes.Type(value = AprilTagPipelineSettings.class)
+    @JsonSubTypes.Type(value = AprilTagPipelineSettings.class),
+    @JsonSubTypes.Type(value = ArucoPipelineSettings.class)
 })
 public class CVPipelineSettings implements Cloneable {
     public int pipelineIndex = 0;
@@ -40,14 +41,14 @@ public class CVPipelineSettings implements Cloneable {
     public String pipelineNickname = "New Pipeline";
     public boolean cameraAutoExposure = false;
     // manual exposure only used if cameraAutoExposure if false
-    public double cameraExposure = 100;
+    public double cameraExposure = 20;
     public int cameraBrightness = 50;
     // Currently only used by a few cameras (notably the zero-copy Pi Camera driver) with the Gain
     // quirk
-    public int cameraGain = 50;
+    public int cameraGain = 75;
     // Currently only used by the zero-copy Pi Camera driver
-    public int cameraRedGain = 18;
-    public int cameraBlueGain = 24;
+    public int cameraRedGain = 11;
+    public int cameraBlueGain = 20;
     public int cameraVideoModeIndex = 0;
     public FrameDivisor streamingFrameDivisor = FrameDivisor.NONE;
     public boolean ledMode = false;

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;
@@ -30,8 +31,9 @@ public class Calibration3dPipelineSettings extends AdvancedPipelineSettings {
 
     public Calibration3dPipelineSettings() {
         super();
-
+        this.cameraAutoExposure = true;
         this.inputShouldShow = true;
         this.outputShouldShow = true;
+        this.streamingFrameDivisor = FrameDivisor.HALF;
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java

@@ -223,7 +223,7 @@ public class ColoredShapePipeline
             collect2dTargetsResult.output.forEach(
                     shape -> {
                         shape.getMinAreaRect().points(rectPoints);
-                        shape.setCorners(Arrays.asList(rectPoints));
+                        shape.setTargetCorners(Arrays.asList(rectPoints));
                     });
             sumPipeNanosElapsed += cornerDetectionResult.nanosElapsed;
 

photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipelineSettings.java

@@ -45,6 +45,7 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
     public ColoredShapePipelineSettings() {
         super();
         pipelineType = PipelineType.ColoredShape;
+        cameraExposure = 20;
     }
 
     @Override

photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java

@@ -77,18 +77,22 @@ public class DriverModePipeline
         // apply pipes
         var inputMat = frame.colorImage.getMat();
 
-        totalNanos += resizeImagePipe.run(inputMat).nanosElapsed;
+        boolean emptyIn = inputMat.empty();
 
-        if (!accelerated) {
-            var rotateImageResult = rotateImagePipe.run(inputMat);
-            totalNanos += rotateImageResult.nanosElapsed;
+        if (!emptyIn) {
+            if (!accelerated) {
+                var rotateImageResult = rotateImagePipe.run(inputMat);
+                totalNanos += rotateImageResult.nanosElapsed;
+            }
+
+            totalNanos += resizeImagePipe.run(inputMat).nanosElapsed;
+
+            var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
+
+            // calculate elapsed nanoseconds
+            totalNanos += draw2dCrosshairResult.nanosElapsed;
         }
 
-        var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
-
-        // calculate elapsed nanoseconds
-        totalNanos += draw2dCrosshairResult.nanosElapsed;
-
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 

photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipelineSettings.java

@@ -31,5 +31,6 @@ public class DriverModePipelineSettings extends CVPipelineSettings {
         pipelineIndex = PipelineManager.DRIVERMODE_INDEX;
         pipelineType = PipelineType.DriverMode;
         inputShouldShow = true;
+        cameraAutoExposure = true;
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java

@@ -24,6 +24,7 @@ import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.DualOffsetValues;
 import org.photonvision.vision.pipe.impl.*;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TargetModel;
 import org.photonvision.vision.target.TrackedTarget;
 
 /**
@@ -37,6 +38,9 @@ public class OutputStreamPipeline {
     private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
     private final Draw2dAprilTagsPipe draw2dAprilTagsPipe = new Draw2dAprilTagsPipe();
     private final Draw3dAprilTagsPipe draw3dAprilTagsPipe = new Draw3dAprilTagsPipe();
+
+    private final Draw2dArucoPipe draw2dArucoPipe = new Draw2dArucoPipe();
+    private final Draw3dArucoPipe draw3dArucoPipe = new Draw3dArucoPipe();
     private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
     private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
 
@@ -65,6 +69,13 @@ public class OutputStreamPipeline {
                         settings.streamingFrameDivisor);
         draw2dAprilTagsPipe.setParams(draw2DAprilTagsParams);
 
+        var draw2DArucoParams =
+                new Draw2dArucoPipe.Draw2dArucoParams(
+                        settings.outputShouldDraw,
+                        settings.outputShowMultipleTargets,
+                        settings.streamingFrameDivisor);
+        draw2dArucoPipe.setParams(draw2DArucoParams);
+
         var draw2dCrosshairParams =
                 new Draw2dCrosshairPipe.Draw2dCrosshairParams(
                         settings.outputShouldDraw,
@@ -92,6 +103,14 @@ public class OutputStreamPipeline {
                         settings.streamingFrameDivisor);
         draw3dAprilTagsPipe.setParams(draw3dAprilTagsParams);
 
+        var draw3dArucoParams =
+                new Draw3dArucoPipe.Draw3dArucoParams(
+                        settings.outputShouldDraw,
+                        frameStaticProperties.cameraCalibration,
+                        TargetModel.k6in_16h5,
+                        settings.streamingFrameDivisor);
+        draw3dArucoPipe.setParams(draw3dArucoParams);
+
         resizeImagePipe.setParams(
                 new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
     }
@@ -110,65 +129,91 @@ public class OutputStreamPipeline {
         boolean inEmpty = inMat.empty();
         if (!inEmpty)
             sumPipeNanosElapsed += pipeProfileNanos[0] = resizeImagePipe.run(inMat).nanosElapsed;
-        sumPipeNanosElapsed += pipeProfileNanos[1] = resizeImagePipe.run(outMat).nanosElapsed;
 
-        // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
-        if (outMat.channels() == 1) {
-            var outputMatPipeResult = outputMatPipe.run(outMat);
-            sumPipeNanosElapsed += pipeProfileNanos[2] = outputMatPipeResult.nanosElapsed;
-        } else {
-            pipeProfileNanos[2] = 0;
-        }
+        boolean outEmpty = outMat.empty();
+        if (!outEmpty)
+            sumPipeNanosElapsed += pipeProfileNanos[1] = resizeImagePipe.run(outMat).nanosElapsed;
 
-        // Draw 2D Crosshair on output
-        var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
-        sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
-
-        if (!(settings instanceof AprilTagPipelineSettings)) {
-            // If we're processing anything other than Apriltags...
-
-            var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
-            sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
-
-            if (settings.solvePNPEnabled) {
-                // Draw 3D Targets on input and output if possible
-                pipeProfileNanos[5] = 0;
-                pipeProfileNanos[6] = 0;
-                pipeProfileNanos[7] = 0;
-
-                var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
+        // Only attempt drawing on a non-empty frame
+        if (!outEmpty) {
+            // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
+            if (outMat.channels() == 1) {
+                var outputMatPipeResult = outputMatPipe.run(outMat);
+                sumPipeNanosElapsed += pipeProfileNanos[2] = outputMatPipeResult.nanosElapsed;
             } else {
-                // Only draw 2d targets
-                pipeProfileNanos[5] = 0;
-
-                var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[6] = draw2dTargetsOnOutput.nanosElapsed;
-
-                pipeProfileNanos[7] = 0;
-                pipeProfileNanos[8] = 0;
+                pipeProfileNanos[2] = 0;
             }
 
-        } else {
-            // If we are doing apriltags...
-            if (settings.solvePNPEnabled) {
-                // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
-                pipeProfileNanos[5] = 0;
-                pipeProfileNanos[6] = 0;
+            // Draw 2D Crosshair on output
+            var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
+            sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
 
-                var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+            if (!(settings instanceof AprilTagPipelineSettings)
+                    && !(settings instanceof ArucoPipelineSettings)) {
+                // If we're processing anything other than Apriltags..
+                var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
+                sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
 
-                pipeProfileNanos[8] = 0;
+                if (settings.solvePNPEnabled) {
+                    // Draw 3D Targets on input and output if possible
+                    pipeProfileNanos[5] = 0;
+                    pipeProfileNanos[6] = 0;
+                    pipeProfileNanos[7] = 0;
 
-            } else {
-                // Draw 2d apriltag markers
-                var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+                    var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
+                } else {
+                    // Only draw 2d targets
+                    pipeProfileNanos[5] = 0;
 
-                pipeProfileNanos[6] = 0;
-                pipeProfileNanos[7] = 0;
-                pipeProfileNanos[8] = 0;
+                    var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[6] = draw2dTargetsOnOutput.nanosElapsed;
+
+                    pipeProfileNanos[7] = 0;
+                    pipeProfileNanos[8] = 0;
+                }
+
+            } else if (settings instanceof AprilTagPipelineSettings) {
+                // If we are doing apriltags...
+                if (settings.solvePNPEnabled) {
+                    // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
+                    pipeProfileNanos[5] = 0;
+                    pipeProfileNanos[6] = 0;
+
+                    var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+
+                    pipeProfileNanos[8] = 0;
+
+                } else {
+                    // Draw 2d apriltag markers
+                    var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+
+                    pipeProfileNanos[6] = 0;
+                    pipeProfileNanos[7] = 0;
+                    pipeProfileNanos[8] = 0;
+                }
+            } else if (settings instanceof ArucoPipelineSettings) {
+                if (settings.solvePNPEnabled) {
+                    // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
+                    pipeProfileNanos[5] = 0;
+                    pipeProfileNanos[6] = 0;
+
+                    var drawOnInputResult = draw3dArucoPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+
+                    pipeProfileNanos[8] = 0;
+
+                } else {
+                    // Draw 2d apriltag markers
+                    var draw2dTargetsOnInput = draw2dArucoPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+
+                    pipeProfileNanos[6] = 0;
+                    pipeProfileNanos[7] = 0;
+                    pipeProfileNanos[8] = 0;
+                }
             }
         }
 

photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineType.java

@@ -23,7 +23,8 @@ public enum PipelineType {
     DriverMode(-1, DriverModePipeline.class),
     Reflective(0, ReflectivePipeline.class),
     ColoredShape(1, ColoredShapePipeline.class),
-    AprilTag(2, AprilTagPipeline.class);
+    AprilTag(2, AprilTagPipeline.class),
+    Aruco(3, ArucoPipeline.class);
 
     public final int baseIndex;
     public final Class clazz;

photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java

@@ -27,5 +27,7 @@ public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
     public ReflectivePipelineSettings() {
         super();
         pipelineType = PipelineType.Reflective;
+        cameraExposure = 6;
+        cameraGain = 20;
     }
 }

photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java

@@ -21,7 +21,11 @@ import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Comparator;
 import java.util.List;
+import org.opencv.aruco.Aruco;
 import org.photonvision.common.configuration.CameraConfiguration;
+import org.photonvision.common.configuration.ConfigManager;
+import org.photonvision.common.dataflow.DataChangeService;
+import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.vision.pipeline.*;
@@ -48,7 +52,7 @@ public class PipelineManager {
      * <br>
      * Used only when switching from any of the built-in pipelines back to a user-created pipeline.
      */
-    private int lastPipelineIndex;
+    private int lastUserPipelineIdx;
 
     /**
      * Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, and all provided
@@ -141,7 +145,7 @@ public class PipelineManager {
      *
      * @return The currently active pipeline.
      */
-    public CVPipeline getCurrentUserPipeline() {
+    public CVPipeline getCurrentPipeline() {
         if (currentPipelineIndex < 0) {
             switch (currentPipelineIndex) {
                 case CAL_3D_INDEX:
@@ -151,23 +155,7 @@ public class PipelineManager {
             }
         }
 
-        var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
-        //        if (currentPipeline.getSettings().pipelineIndex !=
-        // desiredPipelineSettings.pipelineIndex) {
-        //            switch (desiredPipelineSettings.pipelineType) {
-        //                case Reflective:
-        //                    currentPipeline =
-        //                            new ReflectivePipeline((ReflectivePipelineSettings)
-        // desiredPipelineSettings);
-        //                    break;
-        //                case ColoredShape:
-        //                    currentPipeline =
-        //                            new ColoredShapePipeline((ColoredShapePipelineSettings)
-        // desiredPipelineSettings);
-        //                    break;
-        //            }
-        //        }
-
+        // Just return the current user pipeline, we're not on aa built-in one
         return currentUserPipeline;
     }
 
@@ -186,20 +174,21 @@ public class PipelineManager {
      * All externally accessible methods that intend to change the active pipeline MUST go through
      * here to ensure all proper steps are taken.
      *
-     * @param index Index of pipeline to be active
+     * @param newIndex Index of pipeline to be active
      */
-    private void setPipelineInternal(int index) {
-        if (index < 0) {
-            lastPipelineIndex = currentPipelineIndex;
+    private void setPipelineInternal(int newIndex) {
+        if (newIndex < 0 && currentPipelineIndex >= 0) {
+            // Transitioning to a built-in pipe, save off the current user one
+            lastUserPipelineIdx = currentPipelineIndex;
         }
 
-        if (userPipelineSettings.size() - 1 < index) {
+        if (userPipelineSettings.size() - 1 < newIndex) {
             logger.warn("User attempted to set index to non-existent pipeline!");
             return;
         }
 
-        currentPipelineIndex = index;
-        if (index >= 0) {
+        currentPipelineIndex = newIndex;
+        if (newIndex >= 0) {
             var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
             switch (desiredPipelineSettings.pipelineType) {
                 case Reflective:
@@ -217,11 +206,21 @@ public class PipelineManager {
                     currentUserPipeline =
                             new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings);
                     break;
+
+                case Aruco:
+                    logger.debug("Creating Aruco Pipeline");
+                    currentUserPipeline = new ArucoPipeline((ArucoPipelineSettings) desiredPipelineSettings);
+                    break;
                 default:
                     // Can be calib3d or drivermode, both of which are special cases
                     break;
             }
         }
+
+        DataChangeService.getInstance()
+                .publishEvent(
+                        new OutgoingUIEvent<>(
+                                "fullsettings", ConfigManager.getInstance().getConfig().toHashMap()));
     }
 
     /**
@@ -233,7 +232,7 @@ public class PipelineManager {
      */
     public void setCalibrationMode(boolean wantsCalibration) {
         if (!wantsCalibration) calibration3dPipeline.finishCalibration();
-        setPipelineInternal(wantsCalibration ? CAL_3D_INDEX : lastPipelineIndex);
+        setPipelineInternal(wantsCalibration ? CAL_3D_INDEX : lastUserPipelineIdx);
     }
 
     /**
@@ -244,7 +243,7 @@ public class PipelineManager {
      * @param state True to enter driver mode, false to exit driver mode.
      */
     public void setDriverMode(boolean state) {
-        setPipelineInternal(state ? DRIVERMODE_INDEX : lastPipelineIndex);
+        setPipelineInternal(state ? DRIVERMODE_INDEX : lastUserPipelineIdx);
     }
 
     /**
@@ -307,6 +306,12 @@ public class PipelineManager {
                     added.pipelineNickname = nickname;
                     return added;
                 }
+            case Aruco:
+                {
+                    var added = new ArucoPipelineSettings();
+                    added.pipelineNickname = nickname;
+                    return added;
+                }
             default:
                 {
                     logger.error("Got invalid pipeline type: " + type.toString());

photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -104,14 +104,14 @@ public class VisionModule {
         if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
             pipelineManager.userPipelineSettings.forEach(
                     it -> {
-                        if (it.cameraGain == -1) it.cameraGain = 20; // Sane default
+                        if (it.cameraGain == -1) it.cameraGain = 75; // Sane default
                     });
         }
         if (cameraQuirks.hasQuirk(CameraQuirk.AWBGain)) {
             pipelineManager.userPipelineSettings.forEach(
                     it -> {
-                        if (it.cameraRedGain == -1) it.cameraRedGain = 16; // Sane defaults
-                        if (it.cameraBlueGain == -1) it.cameraBlueGain = 16;
+                        if (it.cameraRedGain == -1) it.cameraRedGain = 11; // Sane defaults
+                        if (it.cameraBlueGain == -1) it.cameraBlueGain = 20;
                     });
         }
 
@@ -120,7 +120,7 @@ public class VisionModule {
         this.visionRunner =
                 new VisionRunner(
                         this.visionSource.getFrameProvider(),
-                        this.pipelineManager::getCurrentUserPipeline,
+                        this.pipelineManager::getCurrentPipeline,
                         this::consumeResult,
                         this.cameraQuirks);
         this.streamRunnable = new StreamRunnable(new OutputStreamPipeline());
@@ -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);
@@ -401,7 +401,7 @@ public class VisionModule {
         }
         if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
             // If the gain is disabled for some reason, re-enable it
-            if (pipelineSettings.cameraGain == -1) pipelineSettings.cameraGain = 20;
+            if (pipelineSettings.cameraGain == -1) pipelineSettings.cameraGain = 75;
             visionSource.getSettables().setGain(Math.max(0, pipelineSettings.cameraGain));
         } else {
             pipelineSettings.cameraGain = -1;
@@ -409,8 +409,8 @@ public class VisionModule {
 
         if (cameraQuirks.hasQuirk(CameraQuirk.AWBGain)) {
             // If the AWB gains are disabled for some reason, re-enable it
-            if (pipelineSettings.cameraRedGain == -1) pipelineSettings.cameraRedGain = 16;
-            if (pipelineSettings.cameraBlueGain == -1) pipelineSettings.cameraBlueGain = 16;
+            if (pipelineSettings.cameraRedGain == -1) pipelineSettings.cameraRedGain = 11;
+            if (pipelineSettings.cameraBlueGain == -1) pipelineSettings.cameraBlueGain = 20;
             visionSource.getSettables().setRedGain(Math.max(0, pipelineSettings.cameraRedGain));
             visionSource.getSettables().setBlueGain(Math.max(0, pipelineSettings.cameraBlueGain));
         } else {
@@ -422,6 +422,8 @@ public class VisionModule {
 
         visionSource.getSettables().getConfiguration().currentPipelineIndex =
                 pipelineManager.getCurrentPipelineIndex();
+
+        return true;
     }
 
     private boolean camShouldControlLEDs() {
@@ -578,7 +580,7 @@ public class VisionModule {
     }
 
     public void setTargetModel(TargetModel targetModel) {
-        var settings = pipelineManager.getCurrentUserPipeline().getSettings();
+        var settings = pipelineManager.getCurrentPipeline().getSettings();
         if (settings instanceof ReflectivePipelineSettings) {
             ((ReflectivePipelineSettings) settings).targetModel = targetModel;
             saveAndBroadcastAll();
@@ -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;
@@ -59,7 +60,7 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
 
                 var propName = wsEvent.propertyName;
                 var newPropValue = wsEvent.data;
-                var currentSettings = parentModule.pipelineManager.getCurrentUserPipeline().getSettings();
+                var currentSettings = parentModule.pipelineManager.getCurrentPipeline().getSettings();
 
                 // special case for non-PipelineSetting changes
                 switch (propName) {
@@ -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/target/TargetModel.java

@@ -109,6 +109,14 @@ public enum TargetModel implements Releasable {
                     new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0),
                     new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)),
             Units.inchesToMeters(3.25 * 2)),
+    kAruco6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
+            // do not
+            List.of(
+                    new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0),
+                    new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0),
+                    new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0),
+                    new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)),
+            Units.inchesToMeters(3 * 2)),
     k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
             // do not
             List.of(

photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java

@@ -18,7 +18,10 @@ package org.photonvision.vision.target;
 
 import edu.wpi.first.apriltag.AprilTagDetection;
 import edu.wpi.first.apriltag.AprilTagPoseEstimate;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import java.util.HashMap;
 import java.util.List;
 import org.opencv.core.CvType;
@@ -28,6 +31,7 @@ import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
 import org.opencv.core.RotatedRect;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.*;
 
@@ -37,7 +41,7 @@ public class TrackedTarget implements Releasable {
 
     private MatOfPoint2f m_approximateBoundingPolygon;
 
-    private List<Point> m_targetCorners;
+    private List<Point> m_targetCorners = List.of();
 
     private Point m_targetOffsetPoint;
     private Point m_robotOffsetPoint;
@@ -135,6 +139,59 @@ public class TrackedTarget implements Releasable {
         setCameraRelativeRvec(rvec);
     }
 
+    public TrackedTarget(ArucoDetectionResult result, TargetCalculationParameters params) {
+        m_targetOffsetPoint = new Point(result.getCenterX(), result.getCenterY());
+        m_robotOffsetPoint = new Point();
+
+        m_pitch =
+                TargetCalculations.calculatePitch(
+                        result.getCenterY(), params.cameraCenterPoint.y, params.verticalFocalLength);
+        m_yaw =
+                TargetCalculations.calculateYaw(
+                        result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
+
+        double[] xCorners = result.getxCorners();
+        double[] yCorners = result.getyCorners();
+
+        Point[] cornerPoints =
+                new Point[] {
+                    new Point(xCorners[0], yCorners[0]),
+                    new Point(xCorners[1], yCorners[1]),
+                    new Point(xCorners[2], yCorners[2]),
+                    new Point(xCorners[3], yCorners[3])
+                };
+        m_targetCorners = List.of(cornerPoints);
+        MatOfPoint contourMat = new MatOfPoint(cornerPoints);
+        m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints);
+        m_mainContour = new Contour(contourMat);
+        m_area = m_mainContour.getArea() / params.imageArea * 100;
+        m_fiducialId = result.getId();
+        m_shape = null;
+
+        // TODO implement skew? or just yeet
+
+        var tvec = new Mat(3, 1, CvType.CV_64FC1);
+        tvec.put(0, 0, result.getTvec());
+        setCameraRelativeTvec(tvec);
+
+        var rvec = new Mat(3, 1, CvType.CV_64FC1);
+        rvec.put(0, 0, result.getRvec());
+        setCameraRelativeRvec(rvec);
+
+        {
+            Translation3d translation =
+                    // new Translation3d(tVec.get(0, 0)[0], tVec.get(1, 0)[0], tVec.get(2, 0)[0]);
+                    new Translation3d(result.getTvec()[0], result.getTvec()[1], result.getTvec()[2]);
+            var axisangle =
+                    VecBuilder.fill(result.getRvec()[0], result.getRvec()[1], result.getRvec()[2]);
+            Rotation3d rotation = new Rotation3d(axisangle, axisangle.normF());
+            Transform3d targetPose =
+                    MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation));
+
+            m_bestCameraToTarget3d = targetPose;
+        }
+    }
+
     public void setFiducialId(int id) {
         m_fiducialId = id;
     }
@@ -232,7 +289,7 @@ public class TrackedTarget implements Releasable {
         if (m_cameraRelativeRvec != null) m_cameraRelativeRvec.release();
     }
 
-    public void setCorners(List<Point> targetCorners) {
+    public void setTargetCorners(List<Point> targetCorners) {
         this.m_targetCorners = targetCorners;
     }
 

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

@@ -18,12 +18,12 @@
 package org.photonvision.vision.pipeline;
 
 import edu.wpi.first.math.geometry.Translation3d;
-import java.io.IOException;
 import java.util.stream.Collectors;
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.util.TestUtils;
+import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.frame.provider.FileFrameProvider;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
@@ -32,7 +32,7 @@ import org.photonvision.vision.target.TrackedTarget;
 
 public class AprilTagTest {
     @BeforeEach
-    public void Init() throws IOException {
+    public void Init() {
         TestUtils.loadLibraries();
     }
 
@@ -46,6 +46,7 @@ public class AprilTagTest {
         pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
         pipeline.getSettings().cornerDetectionUseConvexHulls = true;
         pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+        pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
 
         var frameProvider =
                 new FileFrameProvider(
@@ -65,22 +66,21 @@ public class AprilTagTest {
 
         // Draw on input
         var outputPipe = new OutputStreamPipeline();
-        outputPipe.process(
-                pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
+        var ret =
+                outputPipe.process(
+                        pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
 
-        TestUtils.showImage(
-                pipelineResult.inputAndOutputFrame.colorImage.getMat(), "Pipeline output", 999999);
+        TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
 
         // these numbers are not *accurate*, but they are known and expected
         var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
         Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
         Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
-        Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
+        Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2);
 
         var objX = new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY();
         var objY = new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ();
         var objZ = new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX();
-        System.out.printf("Object x %.2f y %.2f z %.2f\n", objX, objY, objZ);
 
         // We expect the object X to be forward, or -X in world space
         Assertions.assertEquals(
@@ -92,6 +92,49 @@ public class AprilTagTest {
         Assertions.assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.1);
     }
 
+    @Test
+    public void testApriltagDistorted() {
+        var pipeline = new AprilTagPipeline();
+
+        pipeline.getSettings().inputShouldShow = true;
+        pipeline.getSettings().outputShouldDraw = true;
+        pipeline.getSettings().solvePNPEnabled = true;
+        pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
+        pipeline.getSettings().cornerDetectionUseConvexHulls = true;
+        pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+        pipeline.getSettings().tagFamily = AprilTagFamily.kTag16h5;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag_corner_1280, false),
+                        TestUtils.WPI2020Image.FOV,
+                        TestUtils.getCoeffs(TestUtils.LIMELIGHT_480P_CAL_FILE, false));
+        frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
+
+        CVPipelineResult pipelineResult;
+        try {
+            pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+            printTestResults(pipelineResult);
+        } catch (RuntimeException e) {
+            // For now, will throw coz rotation3d ctor
+            return;
+        }
+
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        var ret =
+                outputPipe.process(
+                        pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
+
+        TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
+
+        // these numbers are not *accurate*, but they are known and expected
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
+        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);
+    }
+
     private static void printTestResults(CVPipelineResult pipelineResult) {
         double fps = 1000 / pipelineResult.getLatencyMillis();
         System.out.println(

photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java

@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import java.io.IOException;
+import java.util.stream.Collectors;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.photonvision.common.util.TestUtils;
+import org.photonvision.vision.camera.QuirkyCamera;
+import org.photonvision.vision.frame.provider.FileFrameProvider;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TargetModel;
+import org.photonvision.vision.target.TrackedTarget;
+
+public class ArucoPipelineTest {
+    @BeforeEach
+    public void Init() throws IOException {
+        TestUtils.loadLibraries();
+    }
+
+    @Test
+    public void testApriltagFacingCameraAruco() {
+        var pipeline = new ArucoPipeline();
+
+        pipeline.getSettings().inputShouldShow = true;
+        pipeline.getSettings().outputShouldDraw = true;
+        pipeline.getSettings().solvePNPEnabled = true;
+        pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
+        pipeline.getSettings().cornerDetectionUseConvexHulls = true;
+        pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+
+        // pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_16h5_1280, false),
+                        106,
+                        TestUtils.getCoeffs("laptop_1280.json", false));
+        frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
+
+        CVPipelineResult pipelineResult;
+        try {
+            pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+            printTestResults(pipelineResult);
+        } catch (RuntimeException e) {
+            // For now, will throw coz rotation3d ctor
+            return;
+        }
+
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        outputPipe.process(
+                pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
+
+        TestUtils.showImage(
+                pipelineResult.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
+    }
+
+    private static void printTestResults(CVPipelineResult pipelineResult) {
+        double fps = 1000 / pipelineResult.getLatencyMillis();
+        System.out.println(
+                "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)");
+        System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
+        System.out.println(
+                "Found targets at "
+                        + pipelineResult.targets.stream()
+                                .map(TrackedTarget::getBestCameraToTarget3d)
+                                .collect(Collectors.toList()));
+    }
+}

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")
@@ -32,6 +40,12 @@ dependencies {
     implementation wpilibTools.deps.wpilibJava("hal")
     implementation wpilibTools.deps.wpilibJava("ntcore")
     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")
@@ -64,6 +78,8 @@ model {
                 }
             }
             nativeUtils.useRequiredLibrary(it, "wpilib_shared")
+            nativeUtils.useRequiredLibrary(it, "apriltag_shared")
+            nativeUtils.useRequiredLibrary(it, "opencv_shared")
         }
     }
     testSuites {
@@ -78,7 +94,9 @@ model {
             }
 
             nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
+            nativeUtils.useRequiredLibrary(it, "apriltag_shared")
             nativeUtils.useRequiredLibrary(it, "googletest_static")
+            nativeUtils.useRequiredLibrary(it, "opencv_shared")
         }
     }
 }
@@ -120,6 +138,10 @@ task writeCurrentVersion {
 
 build.dependsOn writeCurrentVersion
 
+tasks.withType(Javadoc) {
+    options.encoding = 'UTF-8'
+}
+
 apply from: "publish.gradle"
 
 def testNativeConfigName = 'wpilibTestNatives'
@@ -140,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/generate/photonlib.json.in

@@ -22,7 +22,7 @@
         "windowsx86-64",
         "linuxathena",
         "linuxx86-64",
-        "osxx86-64"
+        "osxuniversal"
       ]
     }
   ],

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,28 +24,38 @@
 
 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;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PubSubOption;
 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;
 import org.photonvision.targeting.PhotonPipelineResult;
 
 /** Represents a camera that is connected to PhotonVision. */
 public class PhotonCamera {
-    protected final NetworkTable rootTable;
+    static final String kTableName = "photonvision";
+
+    protected final NetworkTable cameraTable;
     RawSubscriber rawBytesEntry;
-    BooleanEntry driverModeEntry;
     BooleanPublisher driverModePublisher;
     BooleanSubscriber driverModeSubscriber;
     DoublePublisher latencyMillisEntry;
@@ -56,13 +66,15 @@ public class PhotonCamera {
     DoubleArrayPublisher targetPoseEntry;
     DoublePublisher targetSkewEntry;
     StringSubscriber versionEntry;
-    BooleanPublisher inputSaveImgEntry, outputSaveImgEntry;
-    IntegerEntry pipelineIndexEntry, ledModeEntry;
+    IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
+    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();
@@ -75,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;
@@ -96,6 +113,8 @@ public class PhotonCamera {
 
     Packet packet = new Packet(1);
 
+    private final MultiSubscriber m_topicNameSubscriber;
+
     /**
      * Constructs a PhotonCamera from a root table.
      *
@@ -106,17 +125,35 @@ public class PhotonCamera {
      */
     public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         name = cameraName;
-        var mainTable = instance.getTable("photonvision");
-        this.rootTable = mainTable.getSubTable(cameraName);
-        path = rootTable.getPath();
-        rawBytesEntry = rootTable.getRawTopic("rawBytes").subscribe("rawBytes", new byte[] {});
-        driverModeEntry = rootTable.getBooleanTopic("driverMode").getEntry(false);
-        inputSaveImgEntry = rootTable.getBooleanTopic("inputSaveImgCmd").getEntry(false);
-        outputSaveImgEntry = rootTable.getBooleanTopic("outputSaveImgCmd").getEntry(false);
-        pipelineIndexEntry = rootTable.getIntegerTopic("pipelineIndex").getEntry(0);
-        heartbeatEntry = rootTable.getIntegerTopic("heartbeat").subscribe(-1);
-        ledModeEntry = mainTable.getIntegerTopic("ledMode").getEntry(-1);
-        versionEntry = mainTable.getStringTopic("version").subscribe("");
+        var photonvision_root_table = instance.getTable(kTableName);
+        this.cameraTable = photonvision_root_table.getSubTable(cameraName);
+        path = cameraTable.getPath();
+        rawBytesEntry =
+                cameraTable
+                        .getRawTopic("rawBytes")
+                        .subscribe(
+                                "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
+        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(
+                        instance,
+                        new String[] {"/photonvision/"},
+                        new PubSubOption[] {PubSubOption.topicsOnly(true)});
     }
 
     /**
@@ -162,7 +199,7 @@ public class PhotonCamera {
      * @return Whether the camera is in driver mode.
      */
     public boolean getDriverMode() {
-        return driverModeEntry.get(false);
+        return driverModeSubscriber.get();
     }
 
     /**
@@ -171,7 +208,7 @@ public class PhotonCamera {
      * @param driverMode Whether to set driver mode.
      */
     public void setDriverMode(boolean driverMode) {
-        driverModeEntry.set(driverMode);
+        driverModePublisher.set(driverMode);
     }
 
     /**
@@ -181,7 +218,7 @@ public class PhotonCamera {
      * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
      */
     public void takeInputSnapshot() {
-        inputSaveImgEntry.set(true);
+        inputSaveImgEntry.set(inputSaveImgEntry.get() + 1);
     }
 
     /**
@@ -191,7 +228,7 @@ public class PhotonCamera {
      * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
      */
     public void takeOutputSnapshot() {
-        outputSaveImgEntry.set(true);
+        outputSaveImgEntry.set(outputSaveImgEntry.get() + 1);
     }
 
     /**
@@ -200,7 +237,7 @@ public class PhotonCamera {
      * @return The active pipeline index.
      */
     public int getPipelineIndex() {
-        return (int) pipelineIndexEntry.get(0);
+        return (int) pipelineIndexState.get(0);
     }
 
     /**
@@ -209,7 +246,7 @@ public class PhotonCamera {
      * @param index The active pipeline index.
      */
     public void setPipelineIndex(int index) {
-        pipelineIndexEntry.set(index);
+        pipelineIndexRequest.set(index);
     }
 
     /**
@@ -218,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;
@@ -238,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);
     }
 
     /**
@@ -280,7 +317,21 @@ public class PhotonCamera {
             prevHeartbeatValue = curHeartbeat;
         }
 
-        return ((now - prevHeartbeatChangeTime) > HEARBEAT_DEBOUNCE_SEC);
+        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() {
@@ -292,12 +343,25 @@ 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()) {
-            DriverStation.reportError(
-                    "PhotonVision coprocessor at path " + path + " not found on NetworkTables!", true);
+            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!",
+                        false);
+            } else {
+                DriverStation.reportError(
+                        "PhotonVision coprocessor at path "
+                                + path
+                                + " not found on NetworkTables. Double check that your camera names match!",
+                        true);
+                DriverStation.reportError(
+                        "Found the following PhotonVision cameras on NetworkTables:\n"
+                                + String.join("\n", cameraNames),
+                        false);
+            }
         }
-
         // Check for connection status. Warn if disconnected.
-        if (!isConnected()) {
+        else if (!isConnected()) {
             DriverStation.reportWarning(
                     "PhotonVision coprocessor at path " + path + " is not sending new data.", true);
         }

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

@@ -24,7 +24,9 @@
 
 package org.photonvision;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
@@ -33,9 +35,13 @@ import edu.wpi.first.wpilibj.DriverStation;
 import java.util.ArrayList;
 import java.util.HashSet;
 import java.util.List;
-import java.util.Map;
+import java.util.Optional;
+import java.util.Set;
+import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
+/** @deprecated Use {@link PhotonPoseEstimator} */
+@Deprecated
 public class RobotPoseEstimator {
     /**
      *
@@ -50,7 +56,7 @@ public class RobotPoseEstimator {
      *       calculated
      * </ul>
      */
-    enum PoseStrategy {
+    public enum PoseStrategy {
         LOWEST_AMBIGUITY,
         CLOSEST_TO_CAMERA_HEIGHT,
         CLOSEST_TO_REFERENCE_POSE,
@@ -58,29 +64,27 @@ public class RobotPoseEstimator {
         AVERAGE_BEST_TARGETS
     }
 
-    private Map<Integer, Pose3d> aprilTags;
+    private AprilTagFieldLayout aprilTags;
     private PoseStrategy strategy;
-    private ArrayList<Pair<PhotonCamera, Transform3d>> cameras;
+    private List<Pair<PhotonCamera, Transform3d>> cameras;
     private Pose3d lastPose;
 
     private Pose3d referencePose;
-    private HashSet<Integer> reportedErrors;
+    private Set<Integer> reportedErrors;
 
     /**
      * Create a new RobotPoseEstimator.
      *
-     * <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 Map linking AprilTag IDs to Pose3ds with respect to the FIRST field.
+     * @param aprilTags A WPILib {@link 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 cameras.
+     *     the center of the robot to the camera mount positions (ie, robot ➔ camera).
      */
     public RobotPoseEstimator(
-            Map<Integer, Pose3d> aprilTags,
+            AprilTagFieldLayout aprilTags,
             PoseStrategy strategy,
-            ArrayList<Pair<PhotonCamera, Transform3d>> cameras) {
+            List<Pair<PhotonCamera, Transform3d>> cameras) {
         this.aprilTags = aprilTags;
         this.strategy = strategy;
         this.cameras = cameras;
@@ -91,50 +95,57 @@ public class RobotPoseEstimator {
     /**
      * Update the estimated pose using the selected strategy.
      *
-     * @return The updated estimated pose and the latency in milliseconds
+     * @return The updated estimated pose and the latency in milliseconds. Estimated pose may be null
+     *     if no targets were seen
      */
-    public Pair<Pose3d, Double> update() {
+    public Optional<Pair<Pose3d, Double>> update() {
         if (cameras.isEmpty()) {
             DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false);
-            return Pair.of(lastPose, 0.);
+            return Optional.empty();
         }
-        Pair<Pose3d, Double> pair;
+
+        Pair<Pose3d, Double> pair = getResultFromActiveStrategy();
+
+        if (pair != null) {
+            lastPose = pair.getFirst();
+        }
+
+        return Optional.ofNullable(pair);
+    }
+
+    private Pair<Pose3d, Double> getResultFromActiveStrategy() {
         switch (strategy) {
             case LOWEST_AMBIGUITY:
-                pair = lowestAmbiguityStrategy();
-                lastPose = pair.getFirst();
-                return pair;
+                return lowestAmbiguityStrategy();
             case CLOSEST_TO_CAMERA_HEIGHT:
-                pair = closestToCameraHeightStrategy();
-                lastPose = pair.getFirst();
-                return pair;
+                return closestToCameraHeightStrategy();
             case CLOSEST_TO_REFERENCE_POSE:
-                pair = closestToReferencePoseStrategy();
-                lastPose = pair.getFirst();
-                return pair;
+                return closestToReferencePoseStrategy();
             case CLOSEST_TO_LAST_POSE:
-                referencePose = lastPose;
-                pair = closestToReferencePoseStrategy();
-                lastPose = pair.getFirst();
-                return pair;
+                return closestToLastPoseStrategy();
             case AVERAGE_BEST_TARGETS:
-                pair = averageBestTargetsStrategy();
-                lastPose = pair.getFirst();
-                return pair;
+                return averageBestTargetsStrategy();
             default:
                 DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false);
-                return Pair.of(lastPose, 0.);
+                return null;
         }
     }
 
     private Pair<Pose3d, Double> lowestAmbiguityStrategy() {
-        // Loop over each ambiguity of all the cameras
         int lowestAI = -1;
         int lowestAJ = -1;
         double lowestAmbiguityScore = 10;
+        ArrayList<PhotonPipelineResult> results = new ArrayList<PhotonPipelineResult>(cameras.size());
+
+        // Sample result from each camera
         for (int i = 0; i < cameras.size(); i++) {
             Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            results.add(p.getFirst().getLatestResult());
+        }
+
+        // Loop over each ambiguity of all the cameras
+        for (int i = 0; i < cameras.size(); i++) {
+            List<PhotonTrackedTarget> targets = results.get(i).targets;
             for (int j = 0; j < targets.size(); j++) {
                 if (targets.get(j).getPoseAmbiguity() < lowestAmbiguityScore) {
                     lowestAI = i;
@@ -144,57 +155,45 @@ public class RobotPoseEstimator {
             }
         }
 
-        // No targets, return the last pose
+        // No targets, return null
         if (lowestAI == -1 || lowestAJ == -1) {
-            return Pair.of(lastPose, 0.);
+            return null;
         }
 
         // Pick the lowest and do the heavy calculations
-        PhotonTrackedTarget bestTarget =
-                cameras.get(lowestAI).getFirst().getLatestResult().targets.get(lowestAJ);
+        PhotonTrackedTarget bestTarget = results.get(lowestAI).targets.get(lowestAJ);
 
-        // If the map doesn't contain the ID fail
-        if (!aprilTags.containsKey(bestTarget.getFiducialId())) {
-            if (!reportedErrors.contains(bestTarget.getFiducialId())) {
-                DriverStation.reportError(
-                        "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
-                                + bestTarget.getFiducialId(),
-                        false);
-                reportedErrors.add(bestTarget.getFiducialId());
-            }
-            return Pair.of(lastPose, 0.);
+        Optional<Pose3d> fiducialPose = aprilTags.getTagPose(bestTarget.getFiducialId());
+        if (fiducialPose.isEmpty()) {
+            reportFiducialPoseError(bestTarget.getFiducialId());
+            return null;
         }
 
         return Pair.of(
-                aprilTags
-                        .get(bestTarget.getFiducialId())
+                fiducialPose
+                        .get()
                         .transformBy(bestTarget.getBestCameraToTarget().inverse())
                         .transformBy(cameras.get(lowestAI).getSecond().inverse()),
-                cameras.get(lowestAI).getFirst().getLatestResult().getLatencyMillis());
+                results.get(lowestAI).getLatencyMillis());
     }
 
     private Pair<Pose3d, Double> closestToCameraHeightStrategy() {
-        double smallestHeightDifference = 10e9;
-        double mili = 0;
-        Pose3d pose = lastPose;
+        double smallestHeightDifference = Double.MAX_VALUE;
+        double latency = 0;
+        Pose3d pose = null;
 
         for (int i = 0; i < cameras.size(); i++) {
             Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            var result = p.getFirst().getLatestResult();
+            List<PhotonTrackedTarget> targets = result.targets;
             for (int j = 0; j < targets.size(); j++) {
                 PhotonTrackedTarget target = targets.get(j);
-                // If the map doesn't contain the ID fail
-                if (!aprilTags.containsKey(target.getFiducialId())) {
-                    if (!reportedErrors.contains(target.getFiducialId())) {
-                        DriverStation.reportWarning(
-                                "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
-                                        + target.getFiducialId(),
-                                false);
-                        reportedErrors.add(target.getFiducialId());
-                    }
+                Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
+                if (fiducialPose.isEmpty()) {
+                    reportFiducialPoseError(target.getFiducialId());
                     continue;
                 }
-                Pose3d targetPose = aprilTags.get(target.getFiducialId());
+                Pose3d targetPose = fiducialPose.get();
                 double alternativeDifference =
                         Math.abs(
                                 p.getSecond().getZ()
@@ -205,17 +204,24 @@ public class RobotPoseEstimator {
                                         - targetPose.transformBy(target.getBestCameraToTarget().inverse()).getZ());
                 if (alternativeDifference < smallestHeightDifference) {
                     smallestHeightDifference = alternativeDifference;
-                    pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse());
-                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                    pose =
+                            targetPose
+                                    .transformBy(target.getAlternateCameraToTarget().inverse())
+                                    .transformBy(p.getSecond().inverse());
+                    latency = result.getLatencyMillis();
                 }
                 if (bestDifference < smallestHeightDifference) {
                     smallestHeightDifference = bestDifference;
-                    pose = targetPose.transformBy(target.getBestCameraToTarget().inverse());
-                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                    pose =
+                            targetPose
+                                    .transformBy(target.getBestCameraToTarget().inverse())
+                                    .transformBy(p.getSecond().inverse());
+                    latency = result.getLatencyMillis();
                 }
             }
         }
-        return Pair.of(pose, mili);
+
+        return Pair.of(pose, latency);
     }
 
     private Pair<Pose3d, Double> closestToReferencePoseStrategy() {
@@ -223,51 +229,51 @@ public class RobotPoseEstimator {
             DriverStation.reportError(
                     "[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!",
                     false);
-            return Pair.of(lastPose, 0.);
+            return null;
         }
         double smallestDifference = 10e9;
-        double mili = 0;
-        Pose3d pose = lastPose;
+        double latency = 0;
+        Pose3d pose = null;
         for (int i = 0; i < cameras.size(); i++) {
             Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            var result = p.getFirst().getLatestResult();
+            List<PhotonTrackedTarget> targets = result.targets;
             for (int j = 0; j < targets.size(); j++) {
                 PhotonTrackedTarget target = targets.get(j);
-                // If the map doesn't contain the ID fail
-                if (!aprilTags.containsKey(target.getFiducialId())) {
-                    if (!reportedErrors.contains(target.getFiducialId())) {
-                        DriverStation.reportWarning(
-                                "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
-                                        + target.getFiducialId(),
-                                false);
-                        reportedErrors.add(target.getFiducialId());
-                    }
+                Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
+                if (fiducialPose.isEmpty()) {
+                    reportFiducialPoseError(target.getFiducialId());
                     continue;
                 }
-                Pose3d targetPose = aprilTags.get(target.getFiducialId());
-                double alternativeDifference =
-                        Math.abs(
-                                calculateDifference(
-                                        referencePose,
-                                        targetPose.transformBy(target.getAlternateCameraToTarget().inverse())));
-                double bestDifference =
-                        Math.abs(
-                                calculateDifference(
-                                        referencePose,
-                                        targetPose.transformBy(target.getBestCameraToTarget().inverse())));
+                Pose3d targetPose = fiducialPose.get();
+                Pose3d botBestPose =
+                        targetPose
+                                .transformBy(target.getAlternateCameraToTarget().inverse())
+                                .transformBy(p.getSecond().inverse());
+                Pose3d botAltPose =
+                        targetPose
+                                .transformBy(target.getBestCameraToTarget().inverse())
+                                .transformBy(p.getSecond().inverse());
+                double alternativeDifference = Math.abs(calculateDifference(referencePose, botAltPose));
+                double bestDifference = Math.abs(calculateDifference(referencePose, botBestPose));
                 if (alternativeDifference < smallestDifference) {
                     smallestDifference = alternativeDifference;
-                    pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse());
-                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                    pose = botAltPose;
+                    latency = result.getLatencyMillis();
                 }
                 if (bestDifference < smallestDifference) {
                     smallestDifference = bestDifference;
-                    pose = targetPose.transformBy(target.getBestCameraToTarget().inverse());
-                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                    pose = botBestPose;
+                    latency = result.getLatencyMillis();
                 }
             }
         }
-        return Pair.of(pose, mili);
+        return Pair.of(pose, latency);
+    }
+
+    private Pair<Pose3d, Double> closestToLastPoseStrategy() {
+        setReferencePose(lastPose);
+        return closestToReferencePoseStrategy();
     }
 
     /** Return the average of the best target poses using ambiguity as weight */
@@ -277,34 +283,32 @@ public class RobotPoseEstimator {
         double totalAmbiguity = 0;
         for (int i = 0; i < cameras.size(); i++) {
             Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            var result = p.getFirst().getLatestResult();
+            List<PhotonTrackedTarget> targets = result.targets;
             for (int j = 0; j < targets.size(); j++) {
                 PhotonTrackedTarget target = targets.get(j);
-                // If the map doesn't contain the ID fail
-                if (!aprilTags.containsKey(target.getFiducialId())) {
-                    if (!reportedErrors.contains(target.getFiducialId())) {
-                        DriverStation.reportWarning(
-                                "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
-                                        + target.getFiducialId(),
-                                false);
-                        reportedErrors.add(target.getFiducialId());
-                    }
+                Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
+                if (fiducialPose.isEmpty()) {
+                    reportFiducialPoseError(target.getFiducialId());
                     continue;
                 }
-                Pose3d targetPose = aprilTags.get(target.getFiducialId());
+                Pose3d targetPose = fiducialPose.get();
                 try {
                     totalAmbiguity += 1. / target.getPoseAmbiguity();
                 } catch (ArithmeticException e) {
                     // A total ambiguity of zero exists, using that pose instead!",
                     return Pair.of(
-                            targetPose.transformBy(target.getBestCameraToTarget().inverse()),
-                            p.getFirst().getLatestResult().getLatencyMillis());
+                            targetPose
+                                    .transformBy(target.getBestCameraToTarget().inverse())
+                                    .transformBy(p.getSecond().inverse()),
+                            result.getLatencyMillis());
                 }
                 tempPoses.add(
                         Pair.of(
-                                targetPose.transformBy(target.getBestCameraToTarget().inverse()),
-                                Pair.of(
-                                        target.getPoseAmbiguity(), p.getFirst().getLatestResult().getLatencyMillis())));
+                                targetPose
+                                        .transformBy(target.getBestCameraToTarget().inverse())
+                                        .transformBy(p.getSecond().inverse()),
+                                Pair.of(target.getPoseAmbiguity(), result.getLatencyMillis())));
             }
         }
 
@@ -312,19 +316,23 @@ public class RobotPoseEstimator {
         Rotation3d rotation = new Rotation3d();
         double latency = 0;
 
-        for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
-            try {
-                double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
-                transform = transform.plus(pair.getFirst().getTranslation().times(weight));
-                rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
-                latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
-            } catch (ArithmeticException e) {
-                DriverStation.reportWarning(
-                        "[RobotPoseEstimator] A total ambiguity of zero exists, using that pose instead!",
-                        false);
-                return Pair.of(pair.getFirst(), pair.getSecond().getSecond());
-            }
+        if (tempPoses.isEmpty()) {
+            return null;
         }
+
+        if (totalAmbiguity == 0) {
+            Pose3d p = tempPoses.get(0).getFirst();
+            double l = tempPoses.get(0).getSecond().getSecond();
+            return Pair.of(p, l);
+        }
+
+        for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
+            double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
+            transform = transform.plus(pair.getFirst().getTranslation().times(weight));
+            rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
+            latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
+        }
+
         return Pair.of(new Pose3d(transform, rotation), latency);
     }
 
@@ -338,12 +346,12 @@ public class RobotPoseEstimator {
     }
 
     /** @param aprilTags the aprilTags to set */
-    public void setAprilTags(Map<Integer, Pose3d> aprilTags) {
+    public void setAprilTags(AprilTagFieldLayout aprilTags) {
         this.aprilTags = aprilTags;
     }
 
     /** @return the aprilTags */
-    public Map<Integer, Pose3d> getAprilTags() {
+    public AprilTagFieldLayout getAprilTags() {
         return aprilTags;
     }
 
@@ -372,11 +380,28 @@ public class RobotPoseEstimator {
     }
 
     /**
-     * UPdate the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE
+     * Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
+     *
+     * @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 with CLOSEST_TO_LAST_POSE
      *
      * @param lastPose the lastPose to set
      */
     public void setLastPose(Pose3d lastPose) {
         this.lastPose = lastPose;
     }
+
+    private void reportFiducialPoseError(int fiducialId) {
+        if (!reportedErrors.contains(fiducialId)) {
+            DriverStation.reportError(
+                    "[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;
@@ -36,6 +40,8 @@ import org.photonvision.targeting.PhotonTrackedTarget;
 public class SimPhotonCamera {
     NTTopicSet ts = new NTTopicSet();
     PhotonPipelineResult latestResult;
+    private long heartbeatCounter = 0;
+
     /**
      * Constructs a Simulated PhotonCamera from a root table.
      *
@@ -46,10 +52,32 @@ public class SimPhotonCamera {
      */
     public SimPhotonCamera(NetworkTableInstance instance, String cameraName) {
         ts.removeEntries();
-        ts.subTable = instance.getTable("/photonvision").getSubTable(cameraName);
+        ts.subTable = instance.getTable(PhotonCamera.kTableName).getSubTable(cameraName);
         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.
      *
@@ -134,6 +162,8 @@ public class SimPhotonCamera {
             ts.targetPoseEntry.set(poseData);
         }
 
+        ts.heartbeatPublisher.set(heartbeatCounter++);
+
         latestResult = newResult;
     }
 

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

@@ -24,10 +24,11 @@
 
 package org.photonvision;
 
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 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.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -50,7 +51,7 @@ public class SimVisionSystem {
     int cameraResWidth;
     int cameraResHeight;
     double minTargetArea;
-    Transform3d cameraToRobot;
+    Transform3d robotToCamera;
 
     Field2d dbgField;
     FieldObject2d dbgRobot;
@@ -68,7 +69,8 @@ public class SimVisionSystem {
      * @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the
      *     manufacturer specifications, and/or whatever is configured in the PhotonVision Setting
      *     page.
-     * @param cameraToRobot Transform to move from the camera's mount position to the robot's position
+     * @param robotToCamera Transform to move from the center of the robot to the camera's mount
+     *     position
      * @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and
      *     make it visible. Set to 9000 or more if your vision system does not rely on LED's.
      * @param cameraResWidth Width of your camera's image sensor in pixels
@@ -80,12 +82,12 @@ public class SimVisionSystem {
     public SimVisionSystem(
             String camName,
             double camDiagFOVDegrees,
-            Transform3d cameraToRobot,
+            Transform3d robotToCamera,
             double maxLEDRangeMeters,
             int cameraResWidth,
             int cameraResHeight,
             double minTargetArea) {
-        this.cameraToRobot = cameraToRobot;
+        this.robotToCamera = robotToCamera;
         this.maxLEDRangeMeters = maxLEDRangeMeters;
         this.cameraResWidth = cameraResWidth;
         this.cameraResHeight = cameraResHeight;
@@ -117,17 +119,42 @@ public class SimVisionSystem {
         dbgField
                 .getObject("Target " + Integer.toString(target.targetID))
                 .setPose(target.targetPose.toPose2d());
-        ;
+    }
+
+    /**
+     * Adds all apriltags from the provided {@link AprilTagFieldLayout} as sim vision targets. The
+     * poses added will preserve the tag layout's alliance origin at the time of calling this method.
+     *
+     * @param tagLayout The field tag layout to get Apriltag poses and IDs from
+     */
+    public void addVisionTargets(AprilTagFieldLayout tagLayout) {
+        for (AprilTag tag : tagLayout.getTags()) {
+            addSimVisionTarget(
+                    new SimVisionTarget(
+                            tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
+                            Units.inchesToMeters(6),
+                            Units.inchesToMeters(6),
+                            tag.ID));
+        }
+    }
+
+    /**
+     * 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.
      *
-     * @param newCameraToRobot New Transform from the robot to the camera
+     * @param newRobotToCamera New Transform from the robot to the camera
      */
-    public void moveCamera(Transform3d newCameraToRobot) {
-        this.cameraToRobot = newCameraToRobot;
+    public void moveCamera(Transform3d newRobotToCamera) {
+        this.robotToCamera = newRobotToCamera;
     }
 
     /**
@@ -139,13 +166,7 @@ public class SimVisionSystem {
      *     PhotonVision parameters.
      */
     public void processFrame(Pose2d robotPoseMeters) {
-        var robotPose3d =
-                new Pose3d(
-                        robotPoseMeters.getX(),
-                        robotPoseMeters.getY(),
-                        0.0,
-                        new Rotation3d(0, 0, robotPoseMeters.getRotation().getRadians()));
-        processFrame(robotPose3d);
+        processFrame(new Pose3d(robotPoseMeters));
     }
 
     /**
@@ -157,7 +178,7 @@ public class SimVisionSystem {
      *     PhotonVision parameters.
      */
     public void processFrame(Pose3d robotPoseMeters) {
-        Pose3d cameraPose = robotPoseMeters.transformBy(cameraToRobot.inverse());
+        Pose3d cameraPose = robotPoseMeters.transformBy(robotToCamera);
 
         dbgRobot.setPose(robotPoseMeters.toPose2d());
         dbgCamera.setPose(cameraPose.toPose2d());
@@ -218,6 +239,9 @@ public class SimVisionSystem {
                                         camToTargetTrans,
                                         camToTargetTransAlt,
                                         0.0, // TODO - simulate ambiguity when straight on?
+                                        List.of(
+                                                new TargetCorner(0, 0), new TargetCorner(0, 0),
+                                                new TargetCorner(0, 0), new TargetCorner(0, 0)),
                                         List.of(
                                                 new TargetCorner(0, 0), new TargetCorner(0, 0),
                                                 new TargetCorner(0, 0), new TargetCorner(0, 0))));

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"
@@ -33,35 +34,50 @@
 namespace photonlib {
 
 constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s;
+static const std::vector<std::string_view> PHOTON_PREFIX = {"/photonvision/"};
 
-PhotonCamera::PhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
+PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
                            const std::string_view cameraName)
-    : mainTable(instance->GetTable("photonvision")),
+    : mainTable(instance.GetTable("photonvision")),
       rootTable(mainTable->GetSubTable(cameraName)),
-      rawBytesEntry(rootTable->GetRawTopic("rawBytes").Subscribe("raw", {})),
-      driverModeEntry(rootTable->GetBooleanTopic("driverMode").Publish()),
+      rawBytesEntry(
+          rootTable->GetRawTopic("rawBytes")
+              .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})),
       inputSaveImgEntry(
-          rootTable->GetBooleanTopic("inputSaveImgCmd").Publish()),
+          rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()),
+      inputSaveImgSubscriber(
+          rootTable->GetIntegerTopic("inputSaveImgCmd").Subscribe(0)),
       outputSaveImgEntry(
-          rootTable->GetBooleanTopic("outputSaveImgCmd").Publish()),
-      pipelineIndexEntry(rootTable->GetIntegerTopic("pipelineIndex").Publish()),
-      ledModeEntry(mainTable->GetIntegerTopic("ledMode").Publish()),
+          rootTable->GetIntegerTopic("outputSaveImgCmd").Publish()),
+      outputSaveImgSubscriber(
+          rootTable->GetIntegerTopic("outputSaveImgCmd").Subscribe(0)),
+      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) {}
 
 PhotonCamera::PhotonCamera(const std::string_view cameraName)
-    : PhotonCamera(std::make_shared<nt::NetworkTableInstance>(
-                       nt::NetworkTableInstance::GetDefault()),
-                   cameraName) {}
+    : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
 
 PhotonPipelineResult PhotonCamera::GetLatestResult() {
-  if (test) return testResult;
+  if (test) {
+    return testResult;
+  }
+
   // Prints warning if not connected
   VerifyVersion();
 
@@ -86,33 +102,51 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
 }
 
 void PhotonCamera::SetDriverMode(bool driverMode) {
-  driverModeEntry.Set(driverMode);
+  driverModePublisher.Set(driverMode);
 }
 
-void PhotonCamera::TakeInputSnapshot() { inputSaveImgEntry.Set(true); }
+void PhotonCamera::TakeInputSnapshot() {
+  inputSaveImgEntry.Set(inputSaveImgSubscriber.Get() + 1);
+}
 
-void PhotonCamera::TakeOutputSnapshot() { outputSaveImgEntry.Set(true); }
+void PhotonCamera::TakeOutputSnapshot() {
+  outputSaveImgEntry.Set(outputSaveImgSubscriber.Get() + 1);
+}
 
 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() {
@@ -126,10 +160,29 @@ void PhotonCamera::VerifyVersion() {
   const std::string& versionString = versionEntry.Get("");
   if (versionString.empty()) {
     std::string path_ = path;
-    FRC_ReportError(
-        frc::warn::Warning,
-        "PhotonVision coprocessor at path {} not found on NetworkTables!",
-        path_);
+    std::vector<std::string> cameraNames =
+        rootTable->GetInstance().GetTable("photonvision")->GetSubTables();
+    if (cameraNames.empty()) {
+      FRC_ReportError(frc::warn::Warning,
+                      "Could not find any PhotonVision coprocessors on "
+                      "NetworkTables. Double check that PhotonVision is "
+                      "running, and that your camera is connected!");
+    } else {
+      FRC_ReportError(
+          frc::warn::Warning,
+          "PhotonVision coprocessor at path {} not found on NetworkTables. "
+          "Double check that your camera names match!",
+          path_);
+
+      std::string cameraNameOutString;
+      for (unsigned int i = 0; i < cameraNames.size(); i++) {
+        cameraNameOutString += "\n" + cameraNames[i];
+      }
+      FRC_ReportError(
+          frc::warn::Warning,
+          "Found the following PhotonVision cameras on NetworkTables:{}",
+          cameraNameOutString);
+    }
   } else if (!VersionMatches(versionString)) {
     FRC_ReportError(frc::warn::Warning,
                     "Photon version {} does not match coprocessor version {}!",

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

@@ -30,13 +30,16 @@
 #include <frc/geometry/Translation2d.h>
 #include <wpi/SmallVector.h>
 
+static constexpr const uint8_t MAX_CORNERS = 8;
+
 namespace photonlib {
 
 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> corners)
+    const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners,
+    const std::vector<std::pair<double, double>> detectedCorners)
     : yaw(yaw),
       pitch(pitch),
       area(area),
@@ -45,12 +48,13 @@ PhotonTrackedTarget::PhotonTrackedTarget(
       bestCameraToTarget(pose),
       altCameraToTarget(alternatePose),
       poseAmbiguity(ambiguity),
-      corners(corners) {}
+      minAreaRectCorners(minAreaRectCorners),
+      detectedCorners(detectedCorners) {}
 
 bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
   return other.yaw == yaw && other.pitch == pitch && other.area == area &&
          other.skew == skew && other.bestCameraToTarget == bestCameraToTarget &&
-         other.corners == corners;
+         other.minAreaRectCorners == minAreaRectCorners;
 }
 
 bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
@@ -77,7 +81,16 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
          << target.poseAmbiguity;
 
   for (int i = 0; i < 4; i++) {
-    packet << target.corners[i].first << target.corners[i].second;
+    packet << target.minAreaRectCorners[i].first
+           << target.minAreaRectCorners[i].second;
+  }
+
+  uint8_t num_corners =
+      std::min<uint8_t>(target.detectedCorners.size(), MAX_CORNERS);
+  packet << num_corners;
+  for (size_t i = 0; i < target.detectedCorners.size(); i++) {
+    packet << target.detectedCorners[i].first
+           << target.detectedCorners[i].second;
   }
 
   return packet;
@@ -111,12 +124,21 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
 
   packet >> target.poseAmbiguity;
 
-  target.corners.clear();
+  target.minAreaRectCorners.clear();
+  double first = 0;
+  double second = 0;
   for (int i = 0; i < 4; i++) {
-    double first = 0;
-    double second = 0;
     packet >> first >> second;
-    target.corners.emplace_back(first, second);
+    target.minAreaRectCorners.emplace_back(first, second);
+  }
+
+  uint8_t numCorners = 0;
+  packet >> numCorners;
+  target.detectedCorners.clear();
+  target.detectedCorners.reserve(numCorners);
+  for (size_t i = 0; i < numCorners; i++) {
+    packet >> first >> second;
+    target.detectedCorners.emplace_back(first, second);
   }
 
   return packet;

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

@@ -33,6 +33,7 @@
 #include <vector>
 
 #include <frc/Errors.h>
+#include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/geometry/Pose3d.h>
 #include <frc/geometry/Rotation3d.h>
 #include <frc/geometry/Transform3d.h>
@@ -44,7 +45,7 @@
 
 namespace photonlib {
 RobotPoseEstimator::RobotPoseEstimator(
-    std::map<int, frc::Pose3d> tags, PoseStrategy strat,
+    std::shared_ptr<frc::AprilTagFieldLayout> tags, PoseStrategy strat,
     std::vector<std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>>
         cams)
     : aprilTags(tags),
@@ -53,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();
@@ -72,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;
@@ -84,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;
@@ -106,30 +109,33 @@ 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 =
       cameras[lowestAI].first->GetLatestResult().GetTargets()[lowestAJ];
 
-  if (aprilTags.count(bestTarget.GetFiducialId()) == 0) {
+  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::make_pair(lastPose, units::millisecond_t(0));
+    return std::make_pair(lastPose, units::second_t(0));
   }
 
   return std::make_pair(
-      aprilTags[bestTarget.GetFiducialId()]
+      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) {
@@ -138,13 +144,15 @@ RobotPoseEstimator::ClosestToCameraHeightStrategy() {
         p.first->GetLatestResult().GetTargets();
     for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
       PhotonTrackedTarget target = targets[j];
-      if (aprilTags.count(target.GetFiducialId()) == 0) {
+      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 = aprilTags[target.GetFiducialId()];
+      frc::Pose3d targetPose = fiducialPose.value();
       units::meter_t alternativeDifference = units::math::abs(
           p.second.Z() -
           targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
@@ -156,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) {
@@ -180,13 +189,15 @@ RobotPoseEstimator::ClosestToReferencePoseStrategy() {
         p.first->GetLatestResult().GetTargets();
     for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
       PhotonTrackedTarget target = targets[j];
-      if (aprilTags.count(target.GetFiducialId()) == 0) {
+      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 = aprilTags[target.GetFiducialId()];
+      frc::Pose3d targetPose = fiducialPose.value();
       units::meter_t alternativeDifference =
           units::math::abs(referencePose.Translation().Distance(
               targetPose
@@ -200,39 +211,44 @@ 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];
-      if (aprilTags.count(target.GetFiducialId()) == 0) {
+      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 = aprilTags[target.GetFiducialId()];
+      frc::Pose3d targetPose = fiducialPose.value();
       if (target.GetPoseAmbiguity() == 0) {
         FRC_ReportError(frc::warn::Warning,
                         "Pose ambiguity of zero exists, using that instead!",
@@ -246,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,8 +28,10 @@
 #include <string>
 
 #include <networktables/BooleanTopic.h>
+#include <networktables/DoubleArrayTopic.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/IntegerTopic.h>
+#include <networktables/MultiSubscriber.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
 #include <networktables/RawTopic.h>
@@ -39,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 };
@@ -57,7 +63,7 @@ class PhotonCamera {
    * @param cameraName The name of the camera, as seen in the UI.
    * over.
    */
-  explicit PhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
+  explicit PhotonCamera(nt::NetworkTableInstance instance,
                         const std::string_view cameraName);
 
   /**
@@ -67,6 +73,8 @@ class PhotonCamera {
    */
   explicit PhotonCamera(const std::string_view cameraName);
 
+  PhotonCamera(PhotonCamera&&) = default;
+
   virtual ~PhotonCamera() = default;
 
   /**
@@ -141,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
@@ -165,17 +176,25 @@ class PhotonCamera {
   std::shared_ptr<nt::NetworkTable> mainTable;
   std::shared_ptr<nt::NetworkTable> rootTable;
   nt::RawSubscriber rawBytesEntry;
-  nt::BooleanPublisher driverModeEntry;
-  nt::BooleanPublisher inputSaveImgEntry;
-  nt::BooleanPublisher outputSaveImgEntry;
-  nt::IntegerPublisher pipelineIndexEntry;
-  nt::IntegerPublisher ledModeEntry;
+  nt::IntegerPublisher inputSaveImgEntry;
+  nt::IntegerSubscriber inputSaveImgSubscriber;
+  nt::IntegerPublisher outputSaveImgEntry;
+  nt::IntegerSubscriber outputSaveImgSubscriber;
+  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;
+
   std::string path;
   std::string m_cameraName;
 

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).
@@ -92,10 +93,27 @@ class PhotonTrackedTarget {
   int GetFiducialId() const { return fiducialId; }
 
   /**
-   * Returns the corners of the minimum area rectangle bounding this target.
+   * Return a list of the 4 corners in image space (origin top left, x right, y
+   * down), in no particular order, of the minimum area bounding rectangle of
+   * this target
    */
-  wpi::SmallVector<std::pair<double, double>, 4> GetCorners() const {
-    return corners;
+  wpi::SmallVector<std::pair<double, double>, 4> GetMinAreaRectCorners() const {
+    return minAreaRectCorners;
+  }
+
+  /**
+   * Return a list of the n corners in image space (origin top left, x right, y
+   * down), in no particular order, detected for this target.
+   * For fiducials, the order is known and is always counter-clock wise around
+   * the tag, like so
+   *
+   * -> +X     3 ----- 2
+   * |         |       |
+   * V + Y     |       |
+   *           0 ----- 1
+   */
+  std::vector<std::pair<double, double>> GetDetectedCorners() {
+    return detectedCorners;
   }
 
   /**
@@ -137,6 +155,7 @@ class PhotonTrackedTarget {
   frc::Transform3d bestCameraToTarget;
   frc::Transform3d altCameraToTarget;
   double poseAmbiguity;
-  wpi::SmallVector<std::pair<double, double>, 4> corners;
+  wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners;
+  std::vector<std::pair<double, double>> detectedCorners;
 };
 }  // namespace photonlib

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

@@ -34,6 +34,10 @@
 
 #include "photonlib/PhotonCamera.h"
 
+namespace frc {
+class AprilTagFieldLayout;
+}  // namespace frc
+
 namespace photonlib {
 enum PoseStrategy : int {
   LOWEST_AMBIGUITY,
@@ -44,54 +48,140 @@ 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:
   using map_value_type =
       std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
   using size_type = std::vector<map_value_type>::size_type;
 
- public:
-  explicit RobotPoseEstimator(std::map<int, frc::Pose3d> aprilTags,
-                              PoseStrategy strategy,
-                              std::vector<map_value_type>);
+  /**
+   * Create a new RobotPoseEstimator.
+   *
+   * <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 cameras.
+   */
+  explicit RobotPoseEstimator(
+      std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
+      PoseStrategy strategy, std::vector<map_value_type> cameras);
 
-  std::pair<frc::Pose3d, units::millisecond_t> Update();
-
-  void SetPoseStrategy(PoseStrategy strategy);
-
-  inline void SetReferencePose(frc::Pose3d referencePose) {
-    this->referencePose = referencePose;
+  /**
+   * Get the AprilTagFieldLayout being used by the PositionEstimator.
+   *
+   * @return the AprilTagFieldLayout
+   */
+  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::map<int, frc::Pose3d> aprilTags;
+  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
   PoseStrategy strategy;
   std::vector<map_value_type> cameras;
   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

@@ -37,7 +37,7 @@
 namespace photonlib {
 class SimPhotonCamera : public PhotonCamera {
  public:
-  SimPhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
+  SimPhotonCamera(nt::NetworkTableInstance instance,
                   const std::string& cameraName)
       : PhotonCamera(instance, cameraName) {
     latencyMillisEntry = rootTable->GetEntry("latencyMillis");
@@ -47,15 +47,13 @@ class SimPhotonCamera : public PhotonCamera {
     targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
     targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
     targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
-    rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("raw");
-    versionEntry = instance->GetTable("photonvision")->GetEntry("version");
+    rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("rawBytes");
+    versionEntry = instance.GetTable("photonvision")->GetEntry("version");
     // versionEntry.SetString(PhotonVersion.versionString);
   }
 
   explicit SimPhotonCamera(const std::string& cameraName)
-      : SimPhotonCamera(std::make_shared<nt::NetworkTableInstance>(
-                            nt::NetworkTableInstance::GetDefault()),
-                        cameraName) {}
+      : SimPhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
 
   virtual ~SimPhotonCamera() = default;
 

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