Update cv-select.vue (#719)

Co-authored-by: Mohammad Durrani <46766905+mdurrani808@users.noreply.github.com>

build.gradle

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

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

@@ -676,7 +676,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/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,13 +1,11 @@
 <template>
   <div>
-    <v-select
+    <CVselect
         v-model="selectedFamily"
-        dark
-        color="accent"
-        item-color="secondary"
-        label="Select target family"
-        :items="familyList"
-        @input="handlePipelineUpdate('tagFamily', familyList.indexOf(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"
@@ -78,37 +76,27 @@
 <script>
 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,
+    CVselect,
   },
   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: ['6in (16h5) AprilTag'], //Keep in sync with TargetModel.java
+      familyList: ["AprilTag family 36h11", "AprilTag family 25h9", "AprilTag family 16h5"],
     }
   },
   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];
+        return this.$store.getters.currentPipelineSettings.tagFamily
       },
       set(val) {
-        this.$store.commit("mutatePipeline", {"tagFamily": this.familyList.indexOf(val)})
+        this.$store.commit("mutatePipeline", {"tagFamily": val})
       }
     },
     decimate: {

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-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -189,11 +189,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 +215,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/util/TestUtils.java

@@ -196,14 +196,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() {
@@ -302,6 +306,7 @@ public class TestUtils {
 
     private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json";
     private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json";
+    public static final String LIMELIGHT_480P_CAL_FILE = "limelight_1280_720.json";
 
     public static CameraCalibrationCoefficients getCoeffs(String filename, boolean testMode) {
         try {

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/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/pipeline/AprilTagPipeline.java

@@ -97,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/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;

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/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));
     }
@@ -126,12 +145,12 @@ public class OutputStreamPipeline {
             }
 
             // Draw 2D Crosshair on output
-            var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
+            var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
             sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
 
-            if (!(settings instanceof AprilTagPipelineSettings)) {
-                // If we're processing anything other than Apriltags...
-
+            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;
 
@@ -154,7 +173,7 @@ public class OutputStreamPipeline {
                     pipeProfileNanos[8] = 0;
                 }
 
-            } else {
+            } 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)
@@ -171,6 +190,26 @@ public class OutputStreamPipeline {
                     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/processes/PipelineManager.java

@@ -21,6 +21,7 @@ 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;
@@ -205,6 +206,11 @@ 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;
@@ -300,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/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/test/java/org/photonvision/vision/pipeline/AprilTagTest.java

@@ -18,7 +18,6 @@
 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;
@@ -33,7 +32,7 @@ import org.photonvision.vision.target.TrackedTarget;
 
 public class AprilTagTest {
     @BeforeEach
-    public void Init() throws IOException {
+    public void Init() {
         TestUtils.loadLibraries();
     }
 
@@ -67,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(
@@ -94,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, 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-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/PhotonCamera.java

@@ -31,18 +31,23 @@ import edu.wpi.first.networktables.DoubleArrayPublisher;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.IntegerEntry;
 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.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 {
+    static final String kTableName = "photonvision";
+
     protected final NetworkTable rootTable;
     RawSubscriber rawBytesEntry;
     BooleanEntry driverModeEntry;
@@ -96,6 +101,8 @@ public class PhotonCamera {
 
     Packet packet = new Packet(1);
 
+    private final MultiSubscriber m_topicNameSubscriber;
+
     /**
      * Constructs a PhotonCamera from a root table.
      *
@@ -106,10 +113,14 @@ public class PhotonCamera {
      */
     public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         name = cameraName;
-        var mainTable = instance.getTable("photonvision");
+        var mainTable = instance.getTable(kTableName);
         this.rootTable = mainTable.getSubTable(cameraName);
         path = rootTable.getPath();
-        rawBytesEntry = rootTable.getRawTopic("rawBytes").subscribe("rawBytes", new byte[] {});
+        rawBytesEntry =
+                rootTable
+                        .getRawTopic("rawBytes")
+                        .subscribe(
+                                "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
         driverModeEntry = rootTable.getBooleanTopic("driverMode").getEntry(false);
         inputSaveImgEntry = rootTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
         outputSaveImgEntry = rootTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
@@ -117,6 +128,12 @@ public class PhotonCamera {
         heartbeatEntry = rootTable.getIntegerTopic("heartbeat").subscribe(-1);
         ledModeEntry = mainTable.getIntegerTopic("ledMode").getEntry(-1);
         versionEntry = mainTable.getStringTopic("version").subscribe("");
+
+        m_topicNameSubscriber =
+                new MultiSubscriber(
+                        instance,
+                        new String[] {"/photonvision/"},
+                        new PubSubOption[] {PubSubOption.topicsOnly(true)});
     }
 
     /**
@@ -280,7 +297,7 @@ public class PhotonCamera {
             prevHeartbeatValue = curHeartbeat;
         }
 
-        return ((now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC);
+        return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
     }
 
     private void verifyVersion() {
@@ -292,12 +309,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 = rootTable.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/RobotPoseEstimator.java

@@ -73,14 +73,11 @@ public class RobotPoseEstimator {
     /**
      * 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 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(
             AprilTagFieldLayout aprilTags,
@@ -96,7 +93,7 @@ public class RobotPoseEstimator {
     /**
      * Update the estimated pose using the selected strategy.
      *
-     * @return The updated estimated pose and the latency in milliseconds Estimated pose may be null
+     * @return The updated estimated pose and the latency in milliseconds. Estimated pose may be null
      *     if no targets were seen
      */
     public Optional<Pair<Pose3d, Double>> update() {

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

@@ -36,6 +36,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,7 +48,7 @@ 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();
     }
 
@@ -134,6 +136,8 @@ public class SimPhotonCamera {
             ts.targetPoseEntry.set(poseData);
         }
 
+        ts.heartbeatPublisher.set(heartbeatCounter++);
+
         latestResult = newResult;
     }
 

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

@@ -24,6 +24,8 @@
 
 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;
@@ -49,7 +51,7 @@ public class SimVisionSystem {
     int cameraResWidth;
     int cameraResHeight;
     double minTargetArea;
-    Transform3d cameraToRobot;
+    Transform3d robotToCamera;
 
     Field2d dbgField;
     FieldObject2d dbgRobot;
@@ -67,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
@@ -79,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;
@@ -119,14 +122,31 @@ public class SimVisionSystem {
         ;
     }
 
+    /**
+     * 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));
+        }
+    }
+
     /**
      * 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;
     }
 
     /**
@@ -150,7 +170,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());
@@ -211,6 +231,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/native/cpp/photonlib/PhotonCamera.cpp

@@ -33,12 +33,15 @@
 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", {})),
+      rawBytesEntry(
+          rootTable->GetRawTopic("rawBytes")
+              .Subscribe("raw", {}, {.periodic = 0.01, .sendAll = true})),
       driverModeEntry(rootTable->GetBooleanTopic("driverMode").Publish()),
       inputSaveImgEntry(
           rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()),
@@ -56,13 +59,12 @@ PhotonCamera::PhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
       pipelineIndexSubscriber(
           rootTable->GetIntegerTopic("pipelineIndex").Subscribe(-1)),
       ledModeSubscriber(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
+      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;
@@ -134,10 +136,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/PhotonTrackedTarget.cpp

@@ -30,13 +30,15 @@
 #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)
     : yaw(yaw),
       pitch(pitch),
       area(area),
@@ -45,12 +47,12 @@ PhotonTrackedTarget::PhotonTrackedTarget(
       bestCameraToTarget(pose),
       altCameraToTarget(alternatePose),
       poseAmbiguity(ambiguity),
-      corners(corners) {}
+      minAreaRectCorners(minAreaRectCorners) {}
 
 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 +79,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 +122,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/include/photonlib/PhotonCamera.h

@@ -30,6 +30,7 @@
 #include <networktables/BooleanTopic.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>
@@ -57,7 +58,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);
 
   /**
@@ -178,6 +179,8 @@ class PhotonCamera {
   nt::IntegerSubscriber pipelineIndexSubscriber;
   nt::IntegerSubscriber ledModeSubscriber;
 
+  nt::MultiSubscriber m_topicNameSubscriber;
+
   std::string path;
   std::string m_cameraName;
 

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

@@ -92,10 +92,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 +154,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

@@ -56,10 +56,25 @@ class RobotPoseEstimator {
       std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
   using size_type = std::vector<map_value_type>::size_type;
 
+  /**
+   * Create a new RobotPoseEstimator.
+   *
+   * @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 camera mount positions
+   * (ie, robot -> camera).
+   */
   explicit RobotPoseEstimator(
       std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
       PoseStrategy strategy, std::vector<map_value_type> cameras);
 
+  /**
+   * Update the estimated pose using the selected strategy.
+   *
+   * @return The updated estimated pose and the latency in milliseconds.
+   */
   std::pair<frc::Pose3d, units::millisecond_t> Update();
 
   inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }

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");
@@ -48,14 +48,12 @@ class SimPhotonCamera : public PhotonCamera {
     targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
     targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
     rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("raw");
-    versionEntry = instance->GetTable("photonvision")->GetEntry("version");
+    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/test/java/org/photonvision/PacketTest.java

@@ -47,6 +47,11 @@ class PacketTest {
                         new Transform3d(new Translation3d(), new Rotation3d()),
                         new Transform3d(new Translation3d(), new Rotation3d()),
                         0.25,
+                        List.of(
+                                new TargetCorner(1, 2),
+                                new TargetCorner(3, 4),
+                                new TargetCorner(5, 6),
+                                new TargetCorner(7, 8)),
                         List.of(
                                 new TargetCorner(1, 2),
                                 new TargetCorner(3, 4),
@@ -85,6 +90,11 @@ class PacketTest {
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         0.25,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -99,6 +109,11 @@ class PacketTest {
                                         new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
                                         new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
                                         0.25,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),

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

@@ -94,6 +94,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -108,6 +113,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
                                         new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -127,6 +137,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -166,6 +181,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
                                         new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -180,6 +200,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -199,6 +224,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()),
                                         new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -238,6 +268,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
                                         new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -252,6 +287,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -271,6 +311,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
                                         new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -311,6 +356,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
                                         new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -325,6 +375,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -344,6 +399,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
                                         new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -374,6 +434,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
                                         new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -388,6 +453,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()),
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -406,6 +476,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()),
                                         new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -439,6 +514,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
                                         new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -453,6 +533,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -472,6 +557,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
                                         new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),

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

@@ -182,8 +182,7 @@ class SimVisionSystemTest {
                 new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI));
         var robotToCamera =
                 new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, Math.PI / 4, 0));
-        var sysUnderTest =
-                new SimVisionSystem("Test", 80.0, robotToCamera.inverse(), 99999, 1234, 1234, 0);
+        var sysUnderTest = new SimVisionSystem("Test", 80.0, robotToCamera, 99999, 1234, 1234, 0);
         sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 1736));
 
         var robotPose = new Pose2d(new Translation2d(14.98, 0), Rotation2d.fromDegrees(5));
@@ -247,14 +246,15 @@ class SimVisionSystemTest {
     @ParameterizedTest
     @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999})
     public void testCameraPitch(double testPitch) {
+        testPitch = testPitch * -1;
+
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4));
         final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0));
         var sysUnderTest = new SimVisionSystem("Test", 120.0, new Transform3d(), 99999, 640, 480, 0);
         sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 23));
 
-        // Here, passing in a positive testPitch points the camera downward (since moveCamera takes the
-        // camera->robot transform)
+        // Transform is now robot -> camera
         sysUnderTest.moveCamera(
                 new Transform3d(
                         new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0)));
@@ -263,10 +263,11 @@ class SimVisionSystemTest {
         assertTrue(res.hasTargets());
         var tgt = res.getBestTarget();
 
-        // Since the camera is level with the target, a downward point will mean the target is in the
-        // upper half of the image
-        // which should produce positive pitch.
-        assertEquals(testPitch, tgt.getPitch(), 0.0001);
+        // Since the camera is level with the target, a positive-upward point will mean the target is in
+        // the
+        // lower half of the image
+        // which should produce negative pitch.
+        assertEquals(testPitch * -1, tgt.getPitch(), 0.0001);
     }
 
     private static Stream<Arguments> distCalCParamProvider() {
@@ -309,7 +310,7 @@ class SimVisionSystemTest {
                 new SimVisionSystem(
                         "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!",
                         160.0,
-                        robotToCamera.inverse(),
+                        robotToCamera,
                         99999,
                         640,
                         480,

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

@@ -26,6 +26,7 @@ import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.IntegerSubscriber;
 import edu.wpi.first.networktables.IntegerTopic;
 import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.PubSubOption;
 import edu.wpi.first.networktables.RawPublisher;
 
 /**
@@ -65,7 +66,10 @@ public class NTTopicSet {
     public IntegerPublisher heartbeatPublisher;
 
     public void updateEntries() {
-        rawBytesEntry = subTable.getRawTopic("rawBytes").publish("rawBytes");
+        rawBytesEntry =
+                subTable
+                        .getRawTopic("rawBytes")
+                        .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
 
         pipelineIndexTopic = subTable.getIntegerTopic("pipelineIndex");
         pipelineIndexPublisher = pipelineIndexTopic.publish();

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

@@ -23,11 +23,12 @@ import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import java.util.ArrayList;
 import java.util.List;
-import java.util.Objects;
 import org.photonvision.common.dataflow.structures.Packet;
 
 public class PhotonTrackedTarget {
-    public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7);
+    private static final int MAX_CORNERS = 8;
+    public static final int PACK_SIZE_BYTES =
+            Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS);
 
     private double yaw;
     private double pitch;
@@ -37,7 +38,12 @@ public class PhotonTrackedTarget {
     private Transform3d bestCameraToTarget = new Transform3d();
     private Transform3d altCameraToTarget = new Transform3d();
     private double poseAmbiguity;
-    private List<TargetCorner> targetCorners;
+
+    // Corners from the min-area rectangle bounding the target
+    private List<TargetCorner> minAreaRectCorners;
+
+    // Corners from whatever corner detection method was used
+    private List<TargetCorner> detectedCorners;
 
     public PhotonTrackedTarget() {}
 
@@ -51,8 +57,14 @@ public class PhotonTrackedTarget {
             Transform3d pose,
             Transform3d altPose,
             double ambiguity,
-            List<TargetCorner> corners) {
-        assert corners.size() == 4;
+            List<TargetCorner> minAreaRectCorners,
+            List<TargetCorner> detectedCorners) {
+        assert minAreaRectCorners.size() == 4;
+
+        if (detectedCorners.size() > MAX_CORNERS) {
+            detectedCorners = detectedCorners.subList(0, MAX_CORNERS);
+        }
+
         this.yaw = yaw;
         this.pitch = pitch;
         this.area = area;
@@ -60,7 +72,8 @@ public class PhotonTrackedTarget {
         this.fiducialId = id;
         this.bestCameraToTarget = pose;
         this.altCameraToTarget = altPose;
-        this.targetCorners = corners;
+        this.minAreaRectCorners = minAreaRectCorners;
+        this.detectedCorners = detectedCorners;
         this.poseAmbiguity = ambiguity;
     }
 
@@ -94,11 +107,28 @@ public class PhotonTrackedTarget {
     }
 
     /**
-     * Return a list of the 4 corners in image space (origin top left, x left, y down), in no
+     * 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
      */
-    public List<TargetCorner> getCorners() {
-        return targetCorners;
+    public List<TargetCorner> getMinAreaRectCorners() {
+        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.
+     *
+     * <p>For fiducials, the order is known and is always counter-clock wise around the tag, like so
+     *
+     * <p>spotless:off
+     * -> +X  3 ----- 2
+     * |      |       |
+     * V      |       |
+     * +Y     0 ----- 1
+     * spotless:on
+     */
+    public List<TargetCorner> getDetectedCorners() {
+        return detectedCorners;
     }
 
     /**
@@ -118,21 +148,54 @@ public class PhotonTrackedTarget {
     }
 
     @Override
-    public boolean equals(Object o) {
-        if (this == o) return true;
-        if (o == null || getClass() != o.getClass()) return false;
-        PhotonTrackedTarget that = (PhotonTrackedTarget) o;
-        return Double.compare(that.yaw, yaw) == 0
-                && Double.compare(that.pitch, pitch) == 0
-                && Double.compare(that.area, area) == 0
-                && Objects.equals(bestCameraToTarget, that.bestCameraToTarget)
-                && Objects.equals(altCameraToTarget, that.altCameraToTarget)
-                && Objects.equals(targetCorners, that.targetCorners);
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        long temp;
+        temp = Double.doubleToLongBits(yaw);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(pitch);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(area);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(skew);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        result = prime * result + fiducialId;
+        result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode());
+        result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode());
+        temp = Double.doubleToLongBits(poseAmbiguity);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode());
+        result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.hashCode());
+        return result;
     }
 
     @Override
-    public int hashCode() {
-        return Objects.hash(yaw, pitch, area, bestCameraToTarget, altCameraToTarget);
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj == null) return false;
+        if (getClass() != obj.getClass()) return false;
+        PhotonTrackedTarget other = (PhotonTrackedTarget) obj;
+        if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false;
+        if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false;
+        if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false;
+        if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false;
+        if (fiducialId != other.fiducialId) return false;
+        if (bestCameraToTarget == null) {
+            if (other.bestCameraToTarget != null) return false;
+        } else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false;
+        if (altCameraToTarget == null) {
+            if (other.altCameraToTarget != null) return false;
+        } else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false;
+        if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity))
+            return false;
+        if (minAreaRectCorners == null) {
+            if (other.minAreaRectCorners != null) return false;
+        } else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false;
+        if (detectedCorners == null) {
+            if (other.detectedCorners != null) return false;
+        } else if (!detectedCorners.equals(other.detectedCorners)) return false;
+        return true;
     }
 
     private static Transform3d decodeTransform(Packet packet) {
@@ -158,6 +221,25 @@ public class PhotonTrackedTarget {
         packet.encode(transform.getRotation().getQuaternion().getZ());
     }
 
+    private static void encodeList(Packet packet, List<TargetCorner> list) {
+        packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE));
+        for (int i = 0; i < list.size(); i++) {
+            packet.encode(list.get(i).x);
+            packet.encode(list.get(i).y);
+        }
+    }
+
+    private static List<TargetCorner> decodeList(Packet p) {
+        byte len = p.decodeByte();
+        var ret = new ArrayList<TargetCorner>();
+        for (int i = 0; i < len; i++) {
+            double cx = p.decodeDouble();
+            double cy = p.decodeDouble();
+            ret.add(new TargetCorner(cx, cy));
+        }
+        return ret;
+    }
+
     /**
      * Populates the fields of this class with information from the incoming packet.
      *
@@ -176,13 +258,15 @@ public class PhotonTrackedTarget {
 
         this.poseAmbiguity = packet.decodeDouble();
 
-        this.targetCorners = new ArrayList<>(4);
+        this.minAreaRectCorners = new ArrayList<>(4);
         for (int i = 0; i < 4; i++) {
             double cx = packet.decodeDouble();
             double cy = packet.decodeDouble();
-            targetCorners.add(new TargetCorner(cx, cy));
+            minAreaRectCorners.add(new TargetCorner(cx, cy));
         }
 
+        detectedCorners = decodeList(packet);
+
         return packet;
     }
 
@@ -203,10 +287,12 @@ public class PhotonTrackedTarget {
         packet.encode(poseAmbiguity);
 
         for (int i = 0; i < 4; i++) {
-            packet.encode(targetCorners.get(i).x);
-            packet.encode(targetCorners.get(i).y);
+            packet.encode(minAreaRectCorners.get(i).x);
+            packet.encode(minAreaRectCorners.get(i).y);
         }
 
+        encodeList(packet, detectedCorners);
+
         return packet;
     }
 
@@ -226,7 +312,7 @@ public class PhotonTrackedTarget {
                 + ", cameraToTarget="
                 + bestCameraToTarget
                 + ", targetCorners="
-                + targetCorners
+                + minAreaRectCorners
                 + '}';
     }
 }

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

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

photonlib-cpp-examples/aimandrange/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

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

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

photonlib-cpp-examples/aimattarget/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

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

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

photonlib-cpp-examples/getinrange/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2023Beta",
+    "projectYear": "2023",
     "teamNumber": 6995
 }

photonlib-java-examples/aimandrange/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/aimattarget/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2023Beta",
+    "projectYear": "2023",
     "teamNumber": 6995
 }

photonlib-java-examples/aimattarget/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/apriltagExample/.gitignore

@@ -0,0 +1,162 @@
+# This gitignore has been specially created by the WPILib team.
+# If you remove items from this file, intellisense might break.
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### Java ###
+# Compiled class file
+*.class
+
+# Log file
+*.log
+
+# BlueJ files
+*.ctxt
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.nar
+*.ear
+*.zip
+*.tar.gz
+*.rar
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### macOS ###
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+
+### Windows ###
+# Windows thumbnail cache files
+Thumbs.db
+ehthumbs.db
+ehthumbs_vista.db
+
+# Dump file
+*.stackdump
+
+# Folder config file
+[Dd]esktop.ini
+
+# Recycle Bin used on file shares
+$RECYCLE.BIN/
+
+# Windows Installer files
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# Windows shortcuts
+*.lnk
+
+### Gradle ###
+.gradle
+/build/
+
+# Ignore Gradle GUI config
+gradle-app.setting
+
+# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
+!gradle-wrapper.jar
+
+# Cache of project
+.gradletasknamecache
+
+# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
+# gradle/wrapper/gradle-wrapper.properties
+
+# # VS Code Specific Java Settings
+# DO NOT REMOVE .classpath and .project
+.classpath
+.project
+.settings/
+bin/
+
+# Simulation GUI and other tools window save file
+*-window.json

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

@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": false,
+    "currentLanguage": "java",
+    "projectYear": "2023Beta",
+    "teamNumber": 1736
+}

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

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

photonlib-java-examples/apriltagExample/build.gradle

@@ -0,0 +1,94 @@
+plugins {
+    id "java"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+}
+
+sourceCompatibility = JavaVersion.VERSION_11
+targetCompatibility = JavaVersion.VERSION_11
+
+apply from: "${rootDir}/../shared/examples_common.gradle"
+
+def ROBOT_MAIN_CLASS = "frc.robot.Main"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+    targets {
+        roborio(getTargetTypeClass('RoboRIO')) {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = project.frc.getTeamOrDefault(5940)
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+
+// Set to true to use debug for JNI.
+wpi.java.debugJni = false
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 4.
+dependencies {
+    implementation wpi.java.deps.wpilib()
+    implementation wpi.java.vendor.java()
+
+    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
+    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+
+    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+
+    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+    simulationDebug wpi.sim.enableDebug()
+
+    nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+    nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+    simulationRelease wpi.sim.enableRelease()
+
+    testImplementation 'junit:junit:4.13.1'
+}
+
+// Simulation configuration (e.g. environment variables).
+wpi.sim.addGui().defaultEnabled = true
+wpi.sim.addDriverstation()
+
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+// in order to make them all available at runtime. Also adding the manifest so WPILib
+// knows where to look for our Robot Class.
+jar {
+    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+    duplicatesStrategy = DuplicatesStrategy.INCLUDE
+}
+
+// Configure jar and deploy tasks
+deployArtifact.jarTask = jar
+wpi.java.configureExecutableTasks(jar)
+wpi.java.configureTestTasks(test)
+
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile) {
+    options.compilerArgs.add '-XDstringConcat=inline'
+}

photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.properties

@@ -0,0 +1,5 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists

photonlib-java-examples/apriltagExample/gradlew

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

photonlib-java-examples/apriltagExample/gradlew.bat

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

photonlib-java-examples/apriltagExample/settings.gradle

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

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

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

photonlib-java-examples/apriltagExample/simgui.json

@@ -0,0 +1,34 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Field": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "Shuffleboard": {
+        "open": true
+      },
+      "photonvision": {
+        "OV9281": {
+          "open": true
+        },
+        "USB_Camera": {
+          "open": true
+        },
+        "open": true,
+        "testCamera": {
+          "open": true
+        }
+      }
+    }
+  }
+}

photonlib-java-examples/apriltagExample/src/main/deploy/example.txt

@@ -0,0 +1,3 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
+to get a proper path relative to the deploy directory.

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

@@ -0,0 +1,57 @@
+/*
+ * 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 frc.robot;
+
+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;
+
+public class Constants {
+    static class DriveTrainConstants {
+        static final double kMaxSpeed = 3.0; // meters per second
+        static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
+        static final double kTrackWidth = 0.381 * 2; // meters
+        static final double kWheelRadius = 0.0508; // meters
+        static final int kEncoderResolution = 4096;
+        static final double distancePerPulse = 2 * Math.PI * kWheelRadius / (double) kEncoderResolution;
+    }
+
+    static class FieldConstants {
+        static final double length = Units.feetToMeters(54);
+        static final double width = Units.feetToMeters(27);
+    }
+
+    static class VisionConstants {
+        static final Transform3d robotToCam =
+                new Transform3d(
+                        new Translation3d(0.5, 0.0, 0.5),
+                        new Rotation3d(
+                                0, 0,
+                                0)); // Cam mounted facing forward, half a meter forward of center, half a meter up
+        // from center.
+        static final String cameraName = "YOUR CAMERA NAME";
+    }
+}

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

@@ -0,0 +1,201 @@
+/*
+ * 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 frc.robot;
+
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Constants.DriveTrainConstants;
+
+/** Represents a differential drive style drivetrain. */
+public class Drivetrain {
+    private final MotorController m_leftLeader = new PWMSparkMax(1);
+    private final MotorController m_leftFollower = new PWMSparkMax(2);
+    private final MotorController m_rightLeader = new PWMSparkMax(3);
+    private final MotorController m_rightFollower = new PWMSparkMax(4);
+
+    private final Encoder m_leftEncoder = new Encoder(0, 1);
+    private final Encoder m_rightEncoder = new Encoder(2, 3);
+
+    private final MotorControllerGroup m_leftGroup =
+            new MotorControllerGroup(m_leftLeader, m_leftFollower);
+    private final MotorControllerGroup m_rightGroup =
+            new MotorControllerGroup(m_rightLeader, m_rightFollower);
+
+    private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+    private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
+    private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
+
+    private final DifferentialDriveKinematics m_kinematics =
+            new DifferentialDriveKinematics(Constants.DriveTrainConstants.kTrackWidth);
+
+    public PhotonCameraWrapper pcw;
+
+    /*
+     * Here we use DifferentialDrivePoseEstimator so that we can fuse odometry
+     * readings. The
+     * numbers used below are robot specific, and should be tuned.
+     */
+    private final DifferentialDrivePoseEstimator m_poseEstimator =
+            new DifferentialDrivePoseEstimator(
+                    m_kinematics, m_gyro.getRotation2d(), 0.0, 0.0, new Pose2d());
+
+    // Gains are for example purposes only - must be determined for your own robot!
+    private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+    // Simulation classes help us simulate our robot
+    private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro);
+    private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
+    private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
+    private final Field2d m_fieldSim = new Field2d();
+    private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
+            LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
+    private final DifferentialDrivetrainSim m_drivetrainSimulator =
+            new DifferentialDrivetrainSim(
+                    m_drivetrainSystem,
+                    DCMotor.getCIM(2),
+                    8,
+                    DriveTrainConstants.kTrackWidth,
+                    DriveTrainConstants.kWheelRadius,
+                    null);
+
+    /**
+     * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
+     * gyro.
+     */
+    public Drivetrain() {
+        pcw = new PhotonCameraWrapper();
+
+        m_gyro.reset();
+
+        // We need to invert one side of the drivetrain so that positive voltages
+        // result in both sides moving forward. Depending on how your robot's
+        // gearbox is constructed, you might have to invert the left side instead.
+        m_rightGroup.setInverted(true);
+
+        // Set the distance per pulse for the drive encoders. We can simply use the
+        // distance traveled for one rotation of the wheel divided by the encoder
+        // resolution.
+        m_leftEncoder.setDistancePerPulse(DriveTrainConstants.distancePerPulse);
+        m_rightEncoder.setDistancePerPulse(DriveTrainConstants.distancePerPulse);
+
+        m_leftEncoder.reset();
+        m_rightEncoder.reset();
+
+        SmartDashboard.putData("Field", m_fieldSim);
+    }
+
+    /**
+     * Sets the desired wheel speeds.
+     *
+     * @param speeds The desired wheel speeds.
+     */
+    public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+        final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
+        final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
+
+        final double leftOutput =
+                m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
+        final double rightOutput =
+                m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
+        m_leftGroup.setVoltage(leftOutput + leftFeedforward);
+        m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+    }
+
+    /**
+     * Drives the robot with the given linear velocity and angular velocity.
+     *
+     * @param xSpeed Linear velocity in m/s.
+     * @param rot Angular velocity in rad/s.
+     */
+    public void drive(double xSpeed, double rot) {
+        var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
+        setSpeeds(wheelSpeeds);
+    }
+
+    /** Update our simulation. This should be run every robot loop in simulation. */
+    public void simulationPeriodic() {
+        // To update our simulation, we set motor voltage inputs, update the
+        // simulation, and write the simulated positions and velocities to our
+        // simulated encoder and gyro. We negate the right side so that positive
+        // voltages make the right side move forward.
+        m_drivetrainSimulator.setInputs(
+                m_leftGroup.get() * RobotController.getInputVoltage(),
+                m_rightGroup.get() * RobotController.getInputVoltage());
+        m_drivetrainSimulator.update(0.02);
+
+        m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
+        m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
+        m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
+        m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
+        m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
+    }
+
+    /** Updates the field-relative position. */
+    public void updateOdometry() {
+        m_poseEstimator.update(
+                m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
+
+        // Also apply vision measurements. We use 0.3 seconds in the past as an example
+        // -- on
+        // a real robot, this must be calculated based either on latency or timestamps.
+        Pair<Pose2d, Double> result =
+                pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition());
+        var camPose = result.getFirst();
+        var camPoseObsTime = result.getSecond();
+        if (camPose != null) {
+            m_poseEstimator.addVisionMeasurement(camPose, camPoseObsTime);
+            m_fieldSim.getObject("Cam Est Pos").setPose(camPose);
+        } else {
+            // move it way off the screen to make it disappear
+            m_fieldSim.getObject("Cam Est Pos").setPose(new Pose2d(-100, -100, new Rotation2d()));
+        }
+
+        m_fieldSim.getObject("Actual Pos").setPose(m_drivetrainSimulator.getPose());
+        m_fieldSim.setRobotPose(m_poseEstimator.getEstimatedPosition());
+    }
+}

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

@@ -0,0 +1,45 @@
+/*
+ * 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 frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+    private Main() {}
+
+    /**
+     * Main initialization function. Do not perform any initialization here.
+     *
+     * <p>If you change your main robot class, change the parameter type.
+     */
+    public static void main(String... args) {
+        RobotBase.startRobot(Robot::new);
+    }
+}

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

@@ -0,0 +1,103 @@
+/*
+ * 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 frc.robot;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants.FieldConstants;
+import frc.robot.Constants.VisionConstants;
+import java.util.ArrayList;
+import java.util.Optional;
+import org.photonvision.PhotonCamera;
+import org.photonvision.RobotPoseEstimator;
+import org.photonvision.RobotPoseEstimator.PoseStrategy;
+
+public class PhotonCameraWrapper {
+    public PhotonCamera photonCamera;
+    public RobotPoseEstimator robotPoseEstimator;
+
+    public PhotonCameraWrapper() {
+        // Set up a test arena of two apriltags at the center of each driver station set
+        final AprilTag tag18 =
+                new AprilTag(
+                        18,
+                        new Pose3d(
+                                new Pose2d(
+                                        FieldConstants.length,
+                                        FieldConstants.width / 2.0,
+                                        Rotation2d.fromDegrees(180))));
+        final AprilTag tag01 =
+                new AprilTag(
+                        01,
+                        new Pose3d(new Pose2d(0.0, FieldConstants.width / 2.0, Rotation2d.fromDegrees(0.0))));
+        ArrayList<AprilTag> atList = new ArrayList<AprilTag>();
+        atList.add(tag18);
+        atList.add(tag01);
+
+        // TODO - once 2023 happens, replace this with just loading the 2023 field arrangement
+        AprilTagFieldLayout atfl =
+                new AprilTagFieldLayout(atList, FieldConstants.length, FieldConstants.width);
+
+        // Forward Camera
+        photonCamera =
+                new PhotonCamera(
+                        VisionConstants
+                                .cameraName); // Change the name of your camera here to whatever it is in the
+        // PhotonVision UI.
+
+        // ... Add other cameras here
+
+        // Assemble the list of cameras & mount locations
+        var camList = new ArrayList<Pair<PhotonCamera, Transform3d>>();
+        camList.add(new Pair<PhotonCamera, Transform3d>(photonCamera, VisionConstants.robotToCam));
+
+        robotPoseEstimator =
+                new RobotPoseEstimator(atfl, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, camList);
+    }
+
+    /**
+     * @param estimatedRobotPose The current best guess at robot pose
+     * @return A pair of the fused camera observations to a single Pose2d on the field, and the time
+     *     of the observation. Assumes a planar field and the robot is always firmly on the ground
+     */
+    public Pair<Pose2d, Double> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
+        robotPoseEstimator.setReferencePose(prevEstimatedRobotPose);
+
+        double currentTime = Timer.getFPGATimestamp();
+        Optional<Pair<Pose3d, Double>> result = robotPoseEstimator.update();
+        if (result.isPresent()) {
+            return new Pair<Pose2d, Double>(
+                    result.get().getFirst().toPose2d(), currentTime - result.get().getSecond());
+        } else {
+            return new Pair<Pose2d, Double>(null, 0.0);
+        }
+    }
+}

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

@@ -0,0 +1,94 @@
+/*
+ * 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 frc.robot;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+import frc.robot.Constants.DriveTrainConstants;
+
+public class Robot extends TimedRobot {
+    private XboxController m_controller;
+    private Drivetrain m_drive;
+
+    // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+    private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
+    private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+    @Override
+    public void robotInit() {
+        if (Robot.isSimulation()) {
+            NetworkTableInstance instance = NetworkTableInstance.getDefault();
+            instance.stopServer();
+            // set the NT server if simulating this code.
+            // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]" for coprocessor
+            instance.setServer("localhost");
+            instance.startClient4("myRobot");
+        }
+
+        m_controller = new XboxController(0);
+        m_drive = new Drivetrain();
+    }
+
+    @Override
+    public void robotPeriodic() {
+        m_drive.updateOdometry();
+    }
+
+    @Override
+    public void autonomousInit() {}
+
+    @Override
+    public void autonomousPeriodic() {}
+
+    @Override
+    public void teleopPeriodic() {
+        // Get the x speed. We are inverting this because Xbox controllers return
+        // negative values when we push forward.
+        var joyY = m_controller.getLeftY();
+        if (Math.abs(joyY) < 0.075) {
+            joyY = 0;
+        }
+        final var xSpeed = -m_speedLimiter.calculate(joyY) * DriveTrainConstants.kMaxSpeed;
+
+        // Get the rate of angular rotation. We are inverting this because we want a
+        // positive value when we pull to the left (remember, CCW is positive in
+        // mathematics). Xbox controllers return positive values when you pull to
+        // the right by default.
+        var joyX = m_controller.getRightX();
+        if (Math.abs(joyX) < 0.075) {
+            joyX = 0;
+        }
+        final var rot = -m_rotLimiter.calculate(joyX) * DriveTrainConstants.kMaxAngularSpeed;
+
+        m_drive.drive(xSpeed, rot);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        m_drive.simulationPeriodic();
+    }
+}

photonlib-java-examples/examples.txt

@@ -3,3 +3,4 @@ aimattarget
 getinrange
 simaimandrange
 simposeest
+apriltagExample

photonlib-java-examples/getinrange/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2023Beta",
+    "projectYear": "2023",
     "teamNumber": 6995
 }

photonlib-java-examples/getinrange/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/simaimandrange/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2023Beta",
+    "projectYear": "2023",
     "teamNumber": 6995
 }

photonlib-java-examples/simaimandrange/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/simposeest/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2023Beta",
+    "projectYear": "2023",
     "teamNumber": 6995
 }

photonlib-java-examples/simposeest/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

test-resources/calibration/laptop_1280.json

@@ -0,0 +1,33 @@
+{
+  "resolution": {
+    "width": 1280.0,
+    "height": 720.0
+  },
+  "cameraIntrinsics": {
+    "rows": 3,
+    "cols": 3,
+    "type": 6,
+    "data": [
+      772.8811133362973, 0.0, 658.195907928895, 0.0, 774.614883980155,
+      382.51346080446376, 0.0, 0.0, 1.0
+    ]
+  },
+  "cameraExtrinsics": {
+    "rows": 1,
+    "cols": 5,
+    "type": 6,
+    "data": [
+      0.012379991516584503, -0.02480531511889502, -0.0019890797679669916,
+      0.004880819847116537, 0.011416616592083246
+    ]
+  },
+  "perViewErrors": [
+    0.3638546496153099, 0.320248621678274, 0.5961721612674044,
+    0.36471868370313654, 0.8867304205602674, 0.33170668499682465,
+    1.1510901610887656, 0.3135195406442026, 0.6278864159169598,
+    0.8112949178024729, 0.3712810633477184, 0.9395284037997239,
+    1.3962516818958053, 0.4955041432920438, 0.36239257683589704,
+    0.8017916217152257, 0.3702170636543333
+  ],
+  "standardDeviation": 0.3191763323035045
+}

test-resources/calibration/limelight_1280_720.json

@@ -0,0 +1,52 @@
+{
+  "resolution": {
+    "width": 1280.0,
+    "height": 720.0
+  },
+  "cameraIntrinsics": {
+    "rows": 3,
+    "cols": 3,
+    "type": 6,
+    "data": [
+      1011.3749416937393,
+      0.0,
+      645.4955139388737,
+      0.0,
+      1008.5391755084075,
+      508.32877656020196,
+      0.0,
+      0.0,
+      1.0
+    ]
+  },
+  "cameraExtrinsics": {
+    "rows": 1,
+    "cols": 5,
+    "type": 6,
+    "data": [
+      0.13730101577061535,
+      -0.2904345656989261,
+      8.32475714507539E-4,
+      -3.694397782014239E-4,
+      0.09487962227027584
+    ]
+  },
+  "perViewErrors": [
+    0.4683044439074296,
+    0.3000488602347135,
+    0.3618225279245435,
+    0.3697739589395535,
+    0.3774014417410483,
+    0.3421990728973305,
+    0.26226949827209556,
+    0.2781325613823759,
+    0.35303624904310615,
+    0.4002431454419046,
+    0.364279228602911,
+    0.3616749616010646,
+    0.4458968983384897,
+    0.47429903475059804,
+    0.45590617711230197
+  ],
+  "standardDeviation": 0.06352731093235896
+}