Fix thinclient address in dev builds and move thinclient (#586 )

* Fix thinclient address in dev builds and move thinclient Update USBFrameProvider.java Create index.html Update index.html Null check stream to prevent spam * Update main.yml Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>
Add orthogonalizeRotationMatrix (#587 )
2026-06-28 02:11:40 +00:00 · 2022-11-13 15:51:02 -06:00 · 2022-11-13 14:49:59 -06:00 · 2022-11-13 14:32:49 -06:00 · 2022-11-13 14:07:15 -06:00 · 2022-11-13 13:18:55 -06:00
55 changed files with 1204 additions and 1746 deletions
--- a/photon-client/public/loading.gif
+++ b/photon-client/public/loading.gif
--- a/photon-client/public/thinclient.html
+++ b/photon-client/public/thinclient.html
@@ -0,0 +1,317 @@
+<!DOCTYPE html>
+<html>
+
+<head>
+    <title>ThinClient</title>
+
+    <style>
+        * {
+            margin: 0;
+            padding: 0;
+        }
+        .imgbox {
+            display: grid;
+            height: 100%;
+            width: 100%;
+        }
+        .center-fit {
+
+            width: 90vw;
+            margin: auto;
+        }
+    </style>
+
+</head>
+
+<body>
+    <hr>
+        <div class="imgbox">
+            <img id="streamImg" class="center-fit" src=''>
+        </div>
+    <hr>
+
+    <form id="frm1">
+        Host <input type="text" id="host" value="photonvision.local"><br>
+        Port <input type="text" id="port" value="1181"><br>
+    </form>
+
+    <button>Start Stream</button>
+
+    <script type="module">
+        class WebsocketVideoStream{
+
+            constructor(drawDiv, streamPort, host) {
+
+                this.drawDiv = drawDiv;
+                this.image = document.getElementById(this.drawDiv);
+                this.streamPort = streamPort;
+                this.newStreamPortReq = null;
+                this.serverAddr = "ws://" + host + "/websocket_cameras";
+                this.dispNoStream();
+                this.ws_connect();
+                this.imgData = null;
+                this.imgDataTime = -1;
+                this.imgObjURL = null;
+                this.frameRxCount = 0;
+
+                //Display state machine
+                this.DSM_DISCONNECTED = "DISCONNECTED";
+                this.DSM_WAIT_FOR_VALID_PORT = "WAIT_FOR_VALID_PORT";
+                this.DSM_SUBSCRIBE = "SUBSCRIBE";
+                this.DSM_WAIT_FOR_FIRST_FRAME = "WAIT_FOR_FIRST_FRAME";
+                this.DSM_SHOWING = "SHOWING";
+                this.DSM_RESTART_UNSUBSCRIBE = "UNSUBSCRIBE";
+                this.DSM_RESTART_WAIT = "WAIT_BEFORE_SUBSCRIBE";
+
+                this.dsm_cur_state = this.DSM_DISCONNECTED;
+                this.dsm_prev_state = this.DSM_DISCONNECTED;
+                this.dsm_restart_start_time = window.performance.now();
+
+                requestAnimationFrame(()=>this.animationLoop());
+            }
+
+            dispImageData(){
+                //From https://stackoverflow.com/questions/67507616/set-image-src-from-image-blob/67507685#67507685
+                if(this.imgObjURL != null){
+                    URL.revokeObjectURL(this.imgObjURL)
+                }
+                this.imgObjURL = URL.createObjectURL(this.imgData);
+
+                //Update the image with the new mimetype and image
+                this.image.src = this.imgObjURL;
+            }
+
+            dispNoStream() {
+                this.image.src = "loading.gif";
+            }
+
+            animationLoop(){
+                // Update time metrics
+                var now = window.performance.now();
+                var timeInState  = now - this.dsm_restart_start_time;
+
+                // Save previous state
+                this.dsm_prev_state = this.dsm_cur_state;
+
+                // Evaluate state transitions
+                if(this.serverConnectionActive == false){
+                    //Any state - if the server connection goes false, always transition to disconnected
+                    this.dsm_cur_state = this.DSM_DISCONNECTED;
+                } else {
+                    //Conditional transitions
+                    switch(this.dsm_cur_state) {
+                        case this.DSM_DISCONNECTED:
+                            //Immediately transition to waiting for the first frame
+                            this.dsm_cur_state = this.DSM_WAIT_FOR_VALID_PORT;
+                            break;
+                        case this.DSM_WAIT_FOR_VALID_PORT:
+                            // Wait until the user has configured a valid port
+                            if(this.streamPort > 0){
+                                this.dsm_cur_state = this.DSM_SUBSCRIBE;
+                            } else {
+                                this.dsm_cur_state = this.DSM_WAIT_FOR_VALID_PORT;
+                            }
+                            break;
+                        case this.DSM_SUBSCRIBE:
+                            // Immediately transition after subscriptions is sent
+                            this.dsm_cur_state = this.DSM_WAIT_FOR_FIRST_FRAME;
+                            break;
+                        case this.DSM_WAIT_FOR_FIRST_FRAME:
+                            if(this.imgData != null){
+                                //we got some image data, start showing it
+                                this.dsm_cur_state = this.DSM_SHOWING;
+                            } else if (this.newStreamPortReq != null){
+                                //Stream port requested changed, unsubscribe and restart
+                                this.dsm_cur_state = this.DSM_RESTART_UNSUBSCRIBE;
+                            } else {
+                                this.dsm_cur_state = this.DSM_WAIT_FOR_FIRST_FRAME;
+                            }
+                            break;
+                        case this.DSM_SHOWING:
+                            if((now - this.imgDataTime) > 2500){
+                                //timeout, begin the restart sequence
+                                this.dsm_cur_state = this.DSM_RESTART_UNSUBSCRIBE;
+                            } else if (this.newStreamPortReq != null){
+                                //Stream port requested changed, unsubscribe and restart
+                                this.dsm_cur_state = this.DSM_RESTART_UNSUBSCRIBE;
+                            }  else {
+                                //stay in this state.
+                                this.dsm_cur_state = this.DSM_SHOWING;
+                            }
+                            break;
+                        case this.DSM_RESTART_UNSUBSCRIBE:
+                            //Only should spend one loop in Unsubscribe, immediately transition
+                            this.dsm_cur_state = this.DSM_RESTART_WAIT;
+                            break;
+                        case this.DSM_RESTART_WAIT:
+                            if (timeInState > 250) {
+                                //we've waited long enough, go to try to re-subscribe
+                                this.dsm_cur_state = this.DSM_WAIT_FOR_VALID_PORT;
+                            } else {
+                                //stay in this state.
+                                this.dsm_cur_state = this.DSM_RESTART_WAIT;
+                            }
+                            break;
+                        default:
+                            // Shouldn't get here, default back to init
+                            this.dsm_cur_state = this.DSM_DISCONNECTED;
+                    }
+                }
+
+                //take current-state or state-transition actions
+
+                if(this.dsm_cur_state != this.dsm_prev_state){
+                    //Any state transition
+                    console.log("State Change: " + this.dsm_prev_state + " -> " + this.dsm_cur_state);
+                }
+
+                if(this.dsm_cur_state == this.DSM_SHOWING){
+                    // Currently in SHOWING
+                    this.dispImageData();
+                }
+
+                if(this.dsm_cur_state != this.DSM_SHOWING && this.dsm_prev_state == this.DSM_SHOWING ){
+                    //Any transition out of showing - no stream
+                    this.dispNoStream();
+                }
+
+                if(this.dsm_cur_state == this.DSM_RESTART_UNSUBSCRIBE){
+                    // Currently in UNSUBSCRIBE, do the unsubscribe actions
+                    this.stopStream();
+                    this.dsm_restart_start_time = now;
+                }
+
+                if(this.dsm_cur_state == this.DSM_SUBSCRIBE){
+                    // Currently in SUBSCRIBE, do the subscribe actions
+                    this.startStream();
+                    this.dsm_restart_start_time = now;
+                }
+
+                if(this.dsm_cur_state == this.DSM_WAIT_FOR_VALID_PORT){
+                    // Currently waiting for a vaild port to be requested
+                    if(this.newStreamPortReq != null){
+                        this.streamPort = this.newStreamPortReq;
+                        this.newStreamPortReq = null;
+                    }
+                }
+
+                requestAnimationFrame(()=>this.animationLoop());
+            }
+
+            startStream() {
+                console.log("Subscribing to port " + this.streamPort);
+                this.imgData = null;
+                this.ws.send(JSON.stringify({"cmd": "subscribe", "port":this.streamPort}));
+            }
+
+            stopStream() {
+                console.log("Unsubscribing");
+                this.ws.send(JSON.stringify({"cmd": "unsubscribe"}));
+                this.imgData = null;
+            }
+
+            setPort(streamPort){
+                console.log("Port set to " + streamPort);
+                this.newStreamPortReq = streamPort;
+            }
+
+            ws_onOpen() {
+                // Set the flag allowing general server communication
+                this.serverConnectionActive = true;
+                console.log("Connected!");
+            }
+
+            ws_onClose(e) {
+                //Clear flags to stop server communication
+                this.ws = null;
+                this.serverConnectionActive = false;
+
+                console.log('Camera 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){
+                e; //prevent unused failure
+                this.ws.close();
+            }
+
+            ws_onMessage(e){
+                if(typeof e.data === 'string'){
+                    //string data from host
+                    //TODO - anything to recieve info here? Maybe "avaialble streams?"
+                } else {
+                    if(e.data.size > 0){
+                        //binary data - a frame
+                        this.imgData = e.data;
+                        this.imgDataTime = window.performance.now();
+                        this.frameRxCount++;
+                    } else {
+                        //TODO - server is sending empty frames?
+                    }
+
+                }
+
+            }
+
+            ws_connect() {
+                this.serverConnectionActive = false;
+                this.ws = new WebSocket(this.serverAddr);
+                this.ws.binaryType = "blob";
+                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("Connecting to server " + this.serverAddr);
+            }
+
+            ws_close(){
+                this.ws.close();
+            }
+
+        }
+
+        var stream = null;
+
+        function streamStartRequest() {
+            var host = document.getElementById("host").value + ":5800";
+            var port = document.getElementById("port").value;
+            if(stream == null){
+                stream = new WebsocketVideoStream("streamImg",port,host);
+            } else {
+                stream.setPort(port);
+            }
+
+        }
+
+        // Attach listener
+        document.querySelector('button').addEventListener('click', streamStartRequest);
+
+        // Deal with URLParams, validating inputs
+        const queryString = window.location.search;
+        const urlParams = new URLSearchParams(queryString);
+        const port_in = urlParams.get('port')
+        const host_in = urlParams.get('host')
+        if(port_in != ""){
+            document.getElementById("port").value = port_in;
+        }
+
+        if(host_in != ""){
+            document.getElementById("host").value = host_in;
+        }
+
+        if(port_in != "" & host_in != ""){
+            streamStartRequest(); //we got valid inputs, auto-start the stream
+        }
+
+
+    </script>
+
+</body>
+
+
+</html>
--- a/photon-client/src/App.vue
+++ b/photon-client/src/App.vue
@@ -308,11 +308,7 @@ export default {
                } else if (this.$store.state.settings.hasOwnProperty(key)) {
                    this.$store.commit('mutateSettings', {[key]: value});
                } else {
-                    switch (key) {
-                        default: {
-                            console.error("Unknown message from backend: " + value);
-                        }
-                    }
+                    console.error("Unknown message from backend: " + value);
                }
            },
            toggleCompactMode() {
--- a/photon-client/src/components/common/cv-image.vue
+++ b/photon-client/src/components/common/cv-image.vue
@@ -17,6 +17,7 @@
        data() {
            return {
                seed: 1.0,
+                src: ""
            }
        },
        computed: {
@@ -65,19 +66,18 @@
          disconnected(newVal, oldVal){
            oldVal;
            if(newVal){
-              this.wsStream.stopStream();
+              this.wsStream.setPort(0);
            } else {
-              this.wsStream.startStream();
+              this.wsStream.setPort(this.port);
            }
          }
        },
        mounted() {
          var wsvs = require('../../plugins/WebsocketVideoStream');
-          this.wsStream = new wsvs.WebsocketVideoStream(this.id, this.port, window.location.host);
+          this.wsStream = new wsvs.WebsocketVideoStream(this.id, this.port, this.$address);
        },
        unmounted() {
-          this.wsStream.stopStream();
-          this.wsStream.ws_close();
+          this.wsStream.setPort(0);
        },
        methods: {
            reload() {
--- a/photon-client/src/plugins/WebsocketVideoStream.js
+++ b/photon-client/src/plugins/WebsocketVideoStream.js
@@ -4,95 +4,189 @@ export class WebsocketVideoStream{


    constructor(drawDiv, streamPort, host) {
+        console.log("host " + host + " port " + streamPort)

        this.drawDiv = drawDiv;
        this.image = document.getElementById(this.drawDiv);
        this.streamPort = streamPort;
+        this.newStreamPortReq = null;
        this.serverAddr = "ws://" + host + "/websocket_cameras";
-        this.noStream = false;
-        this.noStreamPrev = false;
-        this.setNoStream();
+        this.dispNoStream();
        this.ws_connect();
        this.imgData = null;
        this.imgDataTime = -1;
        this.imgObjURL = null;
        this.frameRxCount = 0;

+        //Display state machine
+        this.DSM_DISCONNECTED = "DISCONNECTED";
+        this.DSM_WAIT_FOR_VALID_PORT = "WAIT_FOR_VALID_PORT";
+        this.DSM_SUBSCRIBE = "SUBSCRIBE";
+        this.DSM_WAIT_FOR_FIRST_FRAME = "WAIT_FOR_FIRST_FRAME";
+        this.DSM_SHOWING = "SHOWING";
+        this.DSM_RESTART_UNSUBSCRIBE = "UNSUBSCRIBE";
+        this.DSM_RESTART_WAIT = "WAIT_BEFORE_SUBSCRIBE";
+
+        this.dsm_cur_state = this.DSM_DISCONNECTED;
+        this.dsm_prev_state = this.DSM_DISCONNECTED;
+        this.dsm_restart_start_time = window.performance.now();
+
        requestAnimationFrame(()=>this.animationLoop());

    }

+    dispImageData(){
+        //From https://stackoverflow.com/questions/67507616/set-image-src-from-image-blob/67507685#67507685
+        if(this.imgObjURL != null){
+            URL.revokeObjectURL(this.imgObjURL)
+        }
+        this.imgObjURL = URL.createObjectURL(this.imgData);
+
+        //Update the image with the new mimetype and image
+        this.image.src = this.imgObjURL;
+    }
+
+    dispNoStream() {
+        this.image.src = require("../assets/loading.gif");
+    }
+
    animationLoop(){
+        // Update time metrics
        var now = window.performance.now();
+        var timeInState  = now - this.dsm_restart_start_time;

-        if((now - this.imgDataTime) > 2500 && this.imgData != null){
-            //Handle websocket send timeouts by restarting
-            this.setNoStream();
-            this.stopStream();
-            setTimeout(this.startStream.bind(this), 1000); //restart stream one second later
+        // Save previous state
+        this.dsm_prev_state = this.dsm_cur_state;
+
+        // Evaluate state transitions
+        if(this.serverConnectionActive == false){
+            //Any state - if the server connection goes false, always transition to disconnected
+            this.dsm_cur_state = this.DSM_DISCONNECTED;
        } else {
-            if(this.streamPort == null){
-                this.setNoStream();
-            } else if (this.imgData != null) {
-                //From https://stackoverflow.com/questions/67507616/set-image-src-from-image-blob/67507685#67507685
-                if(this.imgObjURL != null){
-                    URL.revokeObjectURL(this.imgObjURL)
-                }
-                this.imgObjURL = URL.createObjectURL(this.imgData);
-
-                //Update the image with the new mimetype and image
-                this.image.src = this.imgObjURL;
-                this.noStream = false;
-
-            } else {
-                //Nothing, hold previous image while waiting for next frame
-            }
+            //Conditional transitions
+            switch(this.dsm_cur_state) {
+                case this.DSM_DISCONNECTED:
+                    //Immediately transition to waiting for the first frame
+                    this.dsm_cur_state = this.DSM_WAIT_FOR_VALID_PORT;
+                    break;
+                case this.DSM_WAIT_FOR_VALID_PORT:
+                    // Wait until the user has configured a valid port
+                    if(this.streamPort > 0){
+                        this.dsm_cur_state = this.DSM_SUBSCRIBE;
+                    } else {
+                        this.dsm_cur_state = this.DSM_WAIT_FOR_VALID_PORT;
+                    }
+                    break;
+                case this.DSM_SUBSCRIBE:
+                    // Immediately transition after subscriptions is sent
+                    this.dsm_cur_state = this.DSM_WAIT_FOR_FIRST_FRAME;
+                    break;
+                case this.DSM_WAIT_FOR_FIRST_FRAME:
+                    if(this.imgData != null){
+                        //we got some image data, start showing it
+                        this.dsm_cur_state = this.DSM_SHOWING;
+                    } else if (this.newStreamPortReq != null){
+                        //Stream port requested changed, unsubscribe and restart
+                        this.dsm_cur_state = this.DSM_RESTART_UNSUBSCRIBE;
+                    } else {
+                        this.dsm_cur_state = this.DSM_WAIT_FOR_FIRST_FRAME;
+                    }
+                    break;
+                case this.DSM_SHOWING:
+                    if((now - this.imgDataTime) > 2500){
+                        //timeout, begin the restart sequence
+                        this.dsm_cur_state = this.DSM_RESTART_UNSUBSCRIBE;
+                    } else if (this.newStreamPortReq != null){
+                        //Stream port requested changed, unsubscribe and restart
+                        this.dsm_cur_state = this.DSM_RESTART_UNSUBSCRIBE;
+                    }  else {
+                        //stay in this state.
+                        this.dsm_cur_state = this.DSM_SHOWING;
+                    }
+                    break;
+                case this.DSM_RESTART_UNSUBSCRIBE:
+                    //Only should spend one loop in Unsubscribe, immediately transition
+                    this.dsm_cur_state = this.DSM_RESTART_WAIT;
+                    break;
+                case this.DSM_RESTART_WAIT:
+                    if (timeInState > 250) {
+                        //we've waited long enough, go to try to re-subscribe
+                        this.dsm_cur_state = this.DSM_WAIT_FOR_VALID_PORT;
+                    } else {
+                        //stay in this state.
+                        this.dsm_cur_state = this.DSM_RESTART_WAIT;
+                    }
+                    break;
+                default:
+                    // Shouldn't get here, default back to init
+                    this.dsm_cur_state = this.DSM_DISCONNECTED;
+              }
        }

+        //take current-state or state-transition actions
+
+        if(this.dsm_cur_state != this.dsm_prev_state){
+            //Any state transition
+            console.log("State Change: " + this.dsm_prev_state + " -> " + this.dsm_cur_state);
+        }
+
+        if(this.dsm_cur_state == this.DSM_SHOWING){
+            // Currently in SHOWING
+            this.dispImageData();
+        }
+
+        if(this.dsm_cur_state != this.DSM_SHOWING && this.dsm_prev_state == this.DSM_SHOWING ){
+            //Any transition out of showing - no stream
+            this.dispNoStream();
+        }
+
+        if(this.dsm_cur_state == this.DSM_RESTART_UNSUBSCRIBE){
+            // Currently in UNSUBSCRIBE, do the unsubscribe actions
+            this.stopStream();
+            this.dsm_restart_start_time = now;
+        }
+
+        if(this.dsm_cur_state == this.DSM_SUBSCRIBE){
+            // Currently in SUBSCRIBE, do the subscribe actions
+            this.startStream();
+            this.dsm_restart_start_time = now;
+        }
+
+        if(this.dsm_cur_state == this.DSM_WAIT_FOR_VALID_PORT){
+            // Currently waiting for a vaild port to be requested
+            if(this.newStreamPortReq != null){
+                this.streamPort = this.newStreamPortReq;
+                this.newStreamPortReq = null;
+            }
+        }

        requestAnimationFrame(()=>this.animationLoop());
    }

-    setNoStream() {
-        this.noStreamPrev = this.noStream;
-        this.noStream = true;
-        if(this.noStreamPrev == false && this.noStream == true){
-            //One-shot background change to preserve animation
-            this.image.src = require("../assets/loading.gif");
-        }
-    }
-
    startStream() {
-        if(this.serverConnectionActive == true && this.streamPort > 0){
-            this.ws.send(JSON.stringify({"cmd": "subscribe", "port":this.streamPort}));
-            this.noStream = false;
-        }
+        console.log("Subscribing to port " + this.streamPort);
+        this.imgData = null;
+        this.ws.send(JSON.stringify({"cmd": "subscribe", "port":this.streamPort}));
    }

    stopStream() {
-        if(this.serverConnectionActive == true && this.streamPort > 0){
-            this.ws.send(JSON.stringify({"cmd": "unsubscribe"}));
-            this.noStream = true;
-        }
+        console.log("Unsubscribing");
+        this.ws.send(JSON.stringify({"cmd": "unsubscribe"}));
+        this.imgData = null;
    }

    setPort(streamPort){
-        this.stopStream();
-        this.frameRxCount = 0;
-        this.streamPort = streamPort;
-        this.startStream();
+        console.log("Port set to " + streamPort);
+        this.newStreamPortReq = streamPort;
    }

    ws_onOpen() {
        // Set the flag allowing general server communication
        this.serverConnectionActive = true;
-        console.log("Connected!");
-        this.startStream();
+        console.log("Camera Websockets Connected!");
    }

    ws_onClose(e) {
-        this.setNoStream();
-
        //Clear flags to stop server communication
        this.ws = null;
        this.serverConnectionActive = false;
@@ -112,6 +206,7 @@ export class WebsocketVideoStream{
    }

    ws_onMessage(e){
+        console.log("Got message from " + this.serverAddr)
        if(typeof e.data === 'string'){
            //string data from host
            //TODO - anything to receive info here? Maybe "available streams?"
@@ -129,6 +224,7 @@ export class WebsocketVideoStream{
    }

    ws_connect() {
+        this.serverConnectionActive = false;
        this.ws = new WebSocket(this.serverAddr);
        this.ws.binaryType = "blob";
        this.ws.onopen = this.ws_onOpen.bind(this);
--- a/photon-client/src/store/index.js
+++ b/photon-client/src/store/index.js
@@ -97,6 +97,8 @@ export default new Vuex.Store({
                    debug: false,
                    refineEdges: true,
                    numIterations: 1,
+                    decisionMargin: 0,
+                    hammingDist: 0,
                }
            }
        ],
--- a/photon-client/src/views/PipelineViews/AprilTagTab.vue
+++ b/photon-client/src/views/PipelineViews/AprilTagTab.vue
@@ -7,16 +7,27 @@
      item-color="secondary"
      label="Select target family"
      :items="familyList"
-      @input="handlePipelineUpdate('tagFamily', targetList.indexOf(selectedModel))"
+      @input="handlePipelineUpdate('tagFamily', familyList.indexOf(selectedFamily))"
+    />
+    <v-select
+      v-model="selectedModel"
+      dark
+      color="accent"
+      item-color="secondary"
+      label="Select a target model"
+      :items="targetList"
+      item-text="name"
+      item-value="data"
+      @input="handlePipelineUpdate('targetModel', targetList.indexOf(selectedModel) + 6)"
    />
    <CVslider
      v-model="decimate"
      class="pt-2"
      slider-cols="8"
      name="Decimate"
-      min="0"
-      max="3"
-      step=".5"
+      min="1"
+      max="8"
+      step="1.0"
      tooltip="Increases FPS at the expense of range by reducing image resolution initially"
      @input="handlePipelineData('decimate')"
    />
@@ -50,6 +61,28 @@
      tooltip="Further refines the apriltag corner position initial estimate, suggested left on"
      @input="handlePipelineData('refineEdges')"
    />
+    <CVslider
+      v-model="hammingDist"
+      class="pt-2 pb-4"
+      slider-cols="8"
+      name="Max error bits"
+      min="0"
+      max="10"
+      step="1"
+      tooltip="Maximum number of error bits to correct; potential tags with more will be thrown out. For smaller tags (like 16h5), set this as low as possible."
+      @input="handlePipelineData('hammingDist')"
+    />
+    <CVslider
+      v-model="decisionMargin"
+      class="pt-2 pb-4"
+      slider-cols="8"
+      name="Decision Margin Cutoff"
+      min="0"
+      max="250"
+      step="1"
+      tooltip="Tags with a 'margin' (decoding quality score) less than this wil be rejected. Increase this to reduce the number of false positive detections"
+      @input="handlePipelineData('decisionMargin')"
+    />
    <CVslider
      v-model="numIterations"
      class="pt-2 pb-4"
@@ -76,10 +109,21 @@
        },
        data() {
            return {
-              familyList: ["tag36h11"],
+              familyList: ["tag36h11", "tag25h9", "tag16h5"],
+              // Selected model is offset (ew) by 6 from the photon ordinal, as we only wanna show the 36h11 and 16h5 options
+              targetList: ['6.5in (36h11) AprilTag', '6in (16h5) AprilTag'], //Keep in sync with TargetModel.java
            }
        },
        computed: {
+          selectedModel: {
+              get() {
+                  let ret = this.$store.getters.currentPipelineSettings.targetModel - 6
+                  return this.targetList[ret];
+              },
+              set(val) {
+                  this.$store.commit("mutatePipeline", {"targetModel": this.targetList.indexOf(val) + 6})
+              }
+          },
          selectedFamily: {
            get() {
                let ret = this.$store.getters.currentPipelineSettings.tagFamily
@@ -97,6 +141,22 @@
                this.$store.commit("mutatePipeline", {"decimate": val});
            }
          },
+          hammingDist: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.hammingDist
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"hammingDist": val});
+            }
+          },
+          decisionMargin: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.decisionMargin
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"decisionMargin": val});
+            }
+          },
          numIterations: {
            get() {
                return this.$store.getters.currentPipelineSettings.numIterations
--- a/photon-client/src/views/PipelineViews/InputTab.vue
+++ b/photon-client/src/views/PipelineViews/InputTab.vue
@@ -107,11 +107,6 @@
        },
        // eslint-disable-next-line vue/require-prop-types
        props: ['value'],
-        data() {
-            return {
-              rawStreamDivisorIndex: 0,
-            }
-        },
        computed: {
            largeBox: {
              get() {
@@ -185,15 +180,22 @@
                    this.$store.commit("mutatePipeline", {"cameraVideoModeIndex": val});

                    this.handlePipelineUpdate("streamingFrameDivisor", this.getNumSkippedStreamDivisors());
-                    this.rawStreamDivisorIndex = 0;
+                    this.$store.commit("mutatePipeline", {"streamingFrameDivisor": 0});
+
+                    // If we don't have 3d mode calibrated at the new resolution either, we should disable it here
+                    // (TODO) This probably belongs in the backend (Matt)
+                    if (!this.$store.getters.isCalibrated) {
+                        this.handlePipelineUpdate("solvePNPEnabled", false);
+                        this.$store.commit("mutatePipeline", {"solvePNPEnabled": false});
+                    }
                }
            },
            streamingFrameDivisor: {
                get() {
-                    return this.rawStreamDivisorIndex;
+                    return this.$store.getters.currentPipelineSettings.streamingFrameDivisor;
                },
                set(val) {
-                    this.rawStreamDivisorIndex = val;
+                    this.$store.commit("mutatePipeline", {"streamingFrameDivisor": val});
                    this.handlePipelineUpdate("streamingFrameDivisor", this.getNumSkippedStreamDivisors() + val);
                }
            },
--- a/photon-client/src/views/PipelineViews/PnPTab.vue
+++ b/photon-client/src/views/PipelineViews/PnPTab.vue
@@ -53,7 +53,7 @@
        },
        data() {
            return {
-                targetList: ['2020 High Goal Outer', '2020 High Goal Inner', '2019 Dual Target', '2020 Power Cell (7in)','2022 Cargo Ball (9.5in)', '2016 High Goal'], //Keep in sync with TargetModel.java
+                targetList: ['2020 High Goal Outer', '2020 High Goal Inner', '2019 Dual Target', '2020 Power Cell (7in)','2022 Cargo Ball (9.5in)', '2016 High Goal', '6.5in (36h11) AprilTag', '6in (16h5) AprilTag'], //Keep in sync with TargetModel.java
                snackbar: {
                    color: "Success",
                    text: ""
@@ -65,7 +65,6 @@
            selectedModel: {
                get() {
                    let ret = this.$store.getters.currentPipelineSettings.targetModel
-                    console.log(ret)
                    return this.targetList[ret];
                },
                set(val) {
--- a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
+++ b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
@@ -18,6 +18,7 @@
 package org.photonvision.common.util.math;

 import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.CoordinateSystem;
@@ -25,10 +26,14 @@ import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Quaternion;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.Arrays;
 import java.util.List;
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import org.ejml.simple.SimpleMatrix;
 import org.opencv.core.Mat;

 public class MathUtils {
@@ -201,4 +206,35 @@ public class MathUtils {
        var axis = rotation.getAxis().times(angle);
        rvecOutput.put(0, 0, axis.getData());
    }
+
+    /**
+     * Orthogonalize an input matrix using a QR decomposition. QR decompositions decompose a
+     * rectangular matrix 'A' such that 'A=QR', where Q is the closest orthogonal matrix to the input,
+     * and R is an upper triangular matrix.
+     */
+    public static Matrix<N3, N3> orthogonalizeRotationMatrix(Matrix<N3, N3> input) {
+        var a = DecompositionFactory_DDRM.qr(3, 3);
+        if (!a.decompose(input.getStorage().getDDRM())) {
+            // best we can do is return the input
+            return input;
+        }
+
+        // Grab results (thanks for this _great_ api, EJML)
+        var Q = new DMatrixRMaj(3, 3);
+        var R = new DMatrixRMaj(3, 3);
+        a.getQ(Q, false);
+        a.getR(R, false);
+
+        // Fix signs in R if they're < 0 so it's close to an identity matrix
+        // (our QR decomposition implementation sometimes flips the signs of columns)
+        for (int colR = 0; colR < 3; ++colR) {
+            if (R.get(colR, colR) < 0) {
+                for (int rowQ = 0; rowQ < 3; ++rowQ) {
+                    Q.set(rowQ, colR, -Q.get(rowQ, colR));
+                }
+            }
+        }
+
+        return new Matrix<>(new SimpleMatrix(Q));
+    }
 }
--- a/photon-core/src/main/java/org/photonvision/vision/apriltag/.styleguide-license
+++ b/photon-core/src/main/java/org/photonvision/vision/apriltag/.styleguide-license
@@ -0,0 +1,25 @@
+/*
+Copyright (c) {year} Photon Vision. 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.
+*/
--- a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetector.java
+++ b/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetector.java
@@ -1,19 +1,28 @@
 /*
- * 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) 2022 Photon Vision. 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.
+*/

 package org.photonvision.vision.apriltag;

--- a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetectorParams.java
+++ b/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetectorParams.java
@@ -1,19 +1,28 @@
 /*
- * 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) 2022 Photon Vision. 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.
+*/

 package org.photonvision.vision.apriltag;

--- a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java
+++ b/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java
@@ -1,19 +1,28 @@
 /*
- * 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) 2022 Photon Vision. 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.
+*/

 package org.photonvision.vision.apriltag;

--- a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagJNI.java
+++ b/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagJNI.java
@@ -1,19 +1,28 @@
 /*
- * 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) 2022 Photon Vision. 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.
+*/

 package org.photonvision.vision.apriltag;

--- a/photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java
+++ b/photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java
@@ -1,19 +1,28 @@
 /*
- * 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) 2022 Photon Vision. 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.
+*/

 package org.photonvision.vision.apriltag;

@@ -24,6 +33,8 @@ import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import java.util.Arrays;

+import org.photonvision.common.util.math.MathUtils;
+
 public class DetectionResult {
    public int getId() {
        return id;
@@ -127,12 +138,12 @@ public class DetectionResult {
        this.poseResult1 =
                new Transform3d(
                        new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]),
-                        new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr)));
+                        new Rotation3d(MathUtils.orthogonalizeRotationMatrix(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr))));
        this.error2 = err2;
        this.poseResult2 =
                new Transform3d(
                        new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]),
-                        new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr)));
+                        new Rotation3d(MathUtils.orthogonalizeRotationMatrix(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr))));
    }

    /**
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
@@ -79,15 +79,25 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
        // target model for the draw 3d targets pipeline to work...

        // for now, hard code tag width based on enum value
-        double tagWidth = 0.16; // guess at 200mm??
+        double tagWidth;
+
+        // This needs
        switch (settings.targetModel) {
            case k200mmAprilTag:
                {
                    tagWidth = Units.inchesToMeters(3.25 * 2);
                    break;
                }
+            case k6in_16h5:
+                {
+                    tagWidth = Units.inchesToMeters(3 * 2);
+                    break;
+                }
            default:
                {
+                    // guess at 200mm?? If it's zero everything breaks, but it should _never_ be zero. Unless
+                    // users select the wrong model...
+                    tagWidth = 0.16;
                    break;
                }
        }
@@ -135,6 +145,10 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel

        targetList = new ArrayList<>();
        for (DetectionResult detection : tagDetectionPipeResult.output) {
+            // TODO this should be in a pipe, not in the top level here (Matt)
+            if (detection.getDecisionMargin() < settings.decisionMargin) continue;
+            if (detection.getHamming() > settings.hammingDist) continue;
+
            // populate the target list
            // Challenge here is that TrackedTarget functions with OpenCV Contour
            TrackedTarget target =
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
@@ -32,6 +32,10 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
    public boolean refineEdges = true;
    public int numIterations = 200;

+    // TODO is this a legit, reasonable default?
+    public int hammingDist = 1;
+    public int decisionMargin = 30;
+
    // 3d settings

    public AprilTagPipelineSettings() {
--- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java
+++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java
@@ -181,17 +181,17 @@ public class PipelineManager {
            var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
            switch (desiredPipelineSettings.pipelineType) {
                case Reflective:
-                    logger.debug("Creatig Reflective pipeline");
+                    logger.debug("Creating Reflective pipeline");
                    currentUserPipeline =
                            new ReflectivePipeline((ReflectivePipelineSettings) desiredPipelineSettings);
                    break;
                case ColoredShape:
-                    logger.debug("Creatig ColoredShape pipeline");
+                    logger.debug("Creating ColoredShape pipeline");
                    currentUserPipeline =
                            new ColoredShapePipeline((ColoredShapePipelineSettings) desiredPipelineSettings);
                    break;
                case AprilTag:
-                    logger.debug("Creatig AprilTag pipeline");
+                    logger.debug("Creating AprilTag pipeline");
                    currentUserPipeline =
                            new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings);
                    break;
--- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
+++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
@@ -17,6 +17,7 @@

 package org.photonvision.vision.processes;

+import edu.wpi.first.cscore.VideoException;
 import edu.wpi.first.math.util.Units;
 import io.javalin.websocket.WsContext;
 import java.util.*;
@@ -388,8 +389,12 @@ public class VisionModule {
        }

        visionSource.getSettables().setExposure(pipelineSettings.cameraExposure);
-        visionSource.getSettables().setAutoExposure(pipelineSettings.cameraAutoExposure);
-
+        try {
+            visionSource.getSettables().setAutoExposure(pipelineSettings.cameraAutoExposure);
+        } catch (VideoException e) {
+            logger.error("Unable to set camera auto exposure!");
+            logger.error(e.toString());
+        }
        if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
            // If the gain is disabled for some reason, re-enable it
            if (pipelineSettings.cameraGain == -1) pipelineSettings.cameraGain = 20;
--- a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java
+++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java
@@ -108,7 +108,15 @@ 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),
                    new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)),
-            Units.inchesToMeters(3.25 * 2));
+            Units.inchesToMeters(3.25 * 2)),
+    k6in_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));

    @JsonIgnore private MatOfPoint3f realWorldTargetCoordinates;
    @JsonIgnore private MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f();
--- a/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStreamManager.java
+++ b/photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStreamManager.java
@@ -69,10 +69,15 @@ public class SocketVideoStreamManager {
    // Indicate a user would like to stop receiving one camera stream
    public void removeSubscription(WsContext user) {
        var port = userSubscriptions.get(user);
-        if (port != null) {
+        if (port != null && port != NO_STREAM_PORT) {
            var stream = streams.get(port);
            userSubscriptions.put(user, NO_STREAM_PORT);
-            stream.removeUser();
+            if (stream != null) {
+                stream.removeUser();
+            }
+        } else {
+            logger.error(
+                    "User attempted to unsubscribe, but had not yet previously subscribed successfully.");
        }
    }

--- a/photon-lib/build.gradle
+++ b/photon-lib/build.gradle
@@ -103,6 +103,8 @@ task generateVendorJson() {
        def read = photonlibFileInput.text.replace('${photon_version}', pubVersion)
        photonlibFileOutput.write(read)
    }
+
+    outputs.upToDateWhen { false }
 }

 build.dependsOn generateVendorJson
--- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
@@ -102,9 +102,14 @@ public class PhotonCamera {

        // Populate packet and create result.
        packet.setData(rawBytesEntry.getRaw(new byte[] {}));
+
        if (packet.getSize() < 1) return ret;
        ret.createFromPacket(packet);

+        // Set the timestamp of the result.
+        // getLatestChange returns in microseconds so we divide by 1e6 to convert to seconds.
+        ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3);
+
        // Return result.
        return ret;
    }
--- a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java
+++ b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java
@@ -61,7 +61,9 @@ public class SimPhotonCamera extends PhotonCamera {
        targetAreaEntry = rootTable.getEntry("targetAreaEntry");
        targetSkewEntry = rootTable.getEntry("targetSkewEntry");
        targetPoseEntry = rootTable.getEntry("targetPoseEntry");
-        versionEntry = rootTable.getEntry("versionEntry");
+
+        // The versionEntry is stored under the main table of <instance>/photonvision
+        versionEntry = instance.getTable("photonvision").getEntry("version");

        // Sets the version string so that it will always match the current version
        versionEntry.setString(PhotonVersion.versionString);
--- a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
+++ b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
@@ -25,9 +25,15 @@
 package org.photonvision;

 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
 import java.util.List;
 import org.photonvision.targeting.PhotonTrackedTarget;
@@ -40,11 +46,14 @@ public class SimVisionSystem {
    double camVertFOVDegrees;
    double cameraHeightOffGroundMeters;
    double maxLEDRangeMeters;
-    double camPitchDegrees;
    int cameraResWidth;
    int cameraResHeight;
    double minTargetArea;
-    Transform2d cameraToRobot;
+    Transform3d cameraToRobot;
+
+    Field2d dbgField;
+    FieldObject2d dbgRobot;
+    FieldObject2d dbgCamera;

    ArrayList<SimVisionTarget> tgtList;

@@ -58,11 +67,7 @@ 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 camPitchDegrees pitch of the camera's view axis back from horizontal. Make this the same
-     *     as whatever is configured in the PhotonVision Setting page.
-     * @param cameraToRobot Pose Transform to move from the camera's mount position to the robot's
-     *     position
-     * @param cameraHeightOffGroundMeters Height of the camera off the ground in meters
+     * @param cameraToRobot Transform to move from the camera's mount position to the robot's 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
@@ -74,16 +79,12 @@ public class SimVisionSystem {
    public SimVisionSystem(
            String camName,
            double camDiagFOVDegrees,
-            double camPitchDegrees,
-            Transform2d cameraToRobot,
-            double cameraHeightOffGroundMeters,
+            Transform3d cameraToRobot,
            double maxLEDRangeMeters,
            int cameraResWidth,
            int cameraResHeight,
            double minTargetArea) {
-        this.camPitchDegrees = camPitchDegrees;
        this.cameraToRobot = cameraToRobot;
-        this.cameraHeightOffGroundMeters = cameraHeightOffGroundMeters;
        this.maxLEDRangeMeters = maxLEDRangeMeters;
        this.cameraResWidth = cameraResWidth;
        this.cameraResHeight = cameraResHeight;
@@ -96,6 +97,11 @@ public class SimVisionSystem {

        cam = new SimPhotonCamera(camName);
        tgtList = new ArrayList<>();
+
+        dbgField = new Field2d();
+        dbgRobot = dbgField.getRobotObject();
+        dbgCamera = dbgField.getObject(camName + " Camera");
+        SmartDashboard.putData(camName + " Sim Field", dbgField);
    }

    /**
@@ -107,6 +113,10 @@ public class SimVisionSystem {
     */
    public void addSimVisionTarget(SimVisionTarget target) {
        tgtList.add(target);
+        dbgField
+                .getObject("Target " + Integer.toString(target.targetID))
+                .setPose(target.targetPose.toPose2d());
+        ;
    }

    /**
@@ -114,14 +124,9 @@ public class SimVisionSystem {
     * turret or some other mobile platform.
     *
     * @param newCameraToRobot New Transform from the robot to the camera
-     * @param newCamHeightMeters New height of the camera off the floor
-     * @param newCamPitchDegrees New pitch of the camera axis back from horizontal
     */
-    public void moveCamera(
-            Transform2d newCameraToRobot, double newCamHeightMeters, double newCamPitchDegrees) {
+    public void moveCamera(Transform3d newCameraToRobot) {
        this.cameraToRobot = newCameraToRobot;
-        this.cameraHeightOffGroundMeters = newCamHeightMeters;
-        this.camPitchDegrees = newCamPitchDegrees;
    }

    /**
@@ -133,46 +138,76 @@ public class SimVisionSystem {
     *     PhotonVision parameters.
     */
    public void processFrame(Pose2d robotPoseMeters) {
-        Pose2d cameraPos = robotPoseMeters.transformBy(cameraToRobot.inverse());
+        var robotPose3d =
+                new Pose3d(
+                        robotPoseMeters.getX(),
+                        robotPoseMeters.getY(),
+                        0.0,
+                        new Rotation3d(0, 0, robotPoseMeters.getRotation().getRadians()));
+        processFrame(robotPose3d);
+    }
+
+    /**
+     * Periodic update. Call this once per frame of image data you wish to process and send to
+     * NetworkTables
+     *
+     * @param robotPoseMeters current pose of the robot in space. Will be used to calculate which
+     *     targets are actually in view, where they are at relative to the robot, and relevant
+     *     PhotonVision parameters.
+     */
+    public void processFrame(Pose3d robotPoseMeters) {
+        Pose3d cameraPose = robotPoseMeters.transformBy(cameraToRobot.inverse());
+
+        dbgRobot.setPose(robotPoseMeters.toPose2d());
+        dbgCamera.setPose(cameraPose.toPose2d());

        ArrayList<PhotonTrackedTarget> visibleTgtList = new ArrayList<>(tgtList.size());

        tgtList.forEach(
                (tgt) -> {
-                    var camToTargetTrans = new Transform2d(cameraPos, tgt.targetPos);
+                    var camToTargetTrans = new Transform3d(cameraPose, tgt.targetPose);
+                    var t = camToTargetTrans.getTranslation();

-                    double distAlongGroundMeters = camToTargetTrans.getTranslation().getNorm();
-                    double distVerticalMeters =
-                            tgt.targetHeightAboveGroundMeters - this.cameraHeightOffGroundMeters;
-                    double distMeters = Math.hypot(distAlongGroundMeters, distVerticalMeters);
+                    // Rough approximation of the alternate solution, which is (so far) always incorrect.
+                    var altTrans =
+                            new Translation3d(
+                                    t.getX(),
+                                    -1.0 * t.getY(),
+                                    t.getZ()); // mirrored across camera axis in Y direction
+                    var altRot = camToTargetTrans.getRotation().times(-1.0); // flipped
+                    var camToTargetTransAlt = new Transform3d(altTrans, altRot);

-                    double area = tgt.tgtAreaMeters2 / getM2PerPx(distAlongGroundMeters);
+                    double distMeters = t.getNorm();
+
+                    double area_px = tgt.tgtAreaMeters2 / getM2PerPx(distMeters);
+
+                    double yawDegrees = Units.radiansToDegrees(Math.atan2(t.getY(), t.getX()));
+
+                    double camHeightAboveGround = cameraPose.getZ();
+                    double tgtHeightAboveGround = tgt.targetPose.getZ();
+                    double camPitchDegrees = Units.radiansToDegrees(cameraPose.getRotation().getY());
+
+                    var transformAlongGround =
+                            new Transform2d(cameraPose.toPose2d(), tgt.targetPose.toPose2d());
+                    double distAlongGround = transformAlongGround.getTranslation().getNorm();

-                    // 2D yaw mode considers the target as a point, and should ignore target rotation.
-                    // Photon reports it in the correct robot reference frame.
-                    // IE: targets to the left of the image should report negative yaw.
-                    double yawDegrees =
-                            -1.0
-                                    * Units.radiansToDegrees(
-                                            Math.atan2(
-                                                    camToTargetTrans.getTranslation().getY(),
-                                                    camToTargetTrans.getTranslation().getX()));
                    double pitchDegrees =
-                            Units.radiansToDegrees(Math.atan2(distVerticalMeters, distAlongGroundMeters))
-                                    - this.camPitchDegrees;
+                            Units.radiansToDegrees(
+                                            Math.atan2((tgtHeightAboveGround - camHeightAboveGround), distAlongGround))
+                                    - camPitchDegrees;

-                    if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area)) {
+                    if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area_px)) {
                        // TODO simulate target corners
                        visibleTgtList.add(
                                new PhotonTrackedTarget(
                                        yawDegrees,
                                        pitchDegrees,
-                                        area,
+                                        area_px,
                                        0.0,
-                                        -1, // TODO fiducial ID
-                                        new Transform3d(),
-                                        new Transform3d(),
-                                        0.25,
+                                        tgt.targetID,
+                                        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))));
--- a/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java
+++ b/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java
@@ -24,34 +24,28 @@

 package org.photonvision;

-import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;

 public class SimVisionTarget {
-    Pose2d targetPos;
+    Pose3d targetPose;
    double targetWidthMeters;
    double targetHeightMeters;
-    double targetHeightAboveGroundMeters;
    double tgtAreaMeters2;
+    int targetID;

    /**
     * Describes a vision target located somewhere on the field that your SimVisionSystem can detect.
     *
-     * @param targetPos Pose2d of the target on the field. Define it such that, if you are standing on
-     *     the middle of the field facing the target, the Y axis points to your left, and the X axis
-     *     points away from you.
-     * @param targetHeightAboveGroundMeters Height of the target above the field plane, in meters.
+     * @param targetPos Pose3d of the target in field-relative coordinates
     * @param targetWidthMeters Width of the outer bounding box of the target in meters.
     * @param targetHeightMeters Pair Height of the outer bounding box of the target in meters.
     */
    public SimVisionTarget(
-            Pose2d targetPos,
-            double targetHeightAboveGroundMeters,
-            double targetWidthMeters,
-            double targetHeightMeters) {
-        this.targetPos = targetPos;
-        this.targetHeightAboveGroundMeters = targetHeightAboveGroundMeters;
+            Pose3d targetPos, double targetWidthMeters, double targetHeightMeters, int targetID) {
+        this.targetPose = targetPos;
        this.targetWidthMeters = targetWidthMeters;
        this.targetHeightMeters = targetHeightMeters;
        this.tgtAreaMeters2 = targetWidthMeters * targetHeightMeters;
+        this.targetID = targetID;
    }
 }
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
@@ -72,6 +72,10 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
  photonlib::Packet packet{bytes};

  packet >> result;
+
+  result.SetTimestamp(units::microsecond_t(rawBytesEntry.GetLastChange()) -
+                      result.GetLatency());
+
  return result;
 }

--- a/photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp
@@ -1,51 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2022 PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include "photonlib/SimPhotonCamera.h"
-
-namespace photonlib {
-
-SimPhotonCamera::SimPhotonCamera(
-    std::shared_ptr<nt::NetworkTableInstance> instance,
-    const std::string& cameraName)
-    : PhotonCamera(instance, cameraName) {}
-
-SimPhotonCamera::SimPhotonCamera(const std::string& cameraName)
-    : PhotonCamera(cameraName) {}
-
-void SimPhotonCamera::SubmitProcessedFrame(
-    units::second_t latency, wpi::span<const PhotonTrackedTarget> tgtList) {
-  if (!GetDriverMode()) {
-    // Clear the current packet.
-    simPacket.Clear();
-
-    // Create the new result and pump it into the packet
-    simPacket << PhotonPipelineResult(latency, tgtList);
-
-    rawBytesEntry.SetRaw(std::string_view{simPacket.GetData().data(),
-                                          simPacket.GetData().size()});
-  }
-}
-
-}  // namespace photonlib
--- a/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp
@@ -1,127 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2022 PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include "photonlib/SimVisionSystem.h"
-
-#include <cmath>
-
-#include <units/angle.h>
-#include <units/length.h>
-#include <wpi/span.h>
-
-namespace photonlib {
-
-SimVisionSystem::SimVisionSystem(const std::string& name,
-                                 units::degree_t camDiagFOV,
-                                 frc::Transform2d cameraToRobot,
-                                 units::meter_t cameraHeightOffGround,
-                                 units::meter_t maxLEDRange, int cameraResWidth,
-                                 int cameraResHeight, double minTargetArea)
-    : cameraToRobot(cameraToRobot),
-      cameraHeightOffGround(cameraHeightOffGround),
-      maxLEDRange(maxLEDRange),
-      cameraResWidth(cameraResWidth),
-      cameraResHeight(cameraResHeight),
-      minTargetArea(minTargetArea) {
-  double hypotPixels = std::hypot(cameraResWidth, cameraResHeight);
-  camHorizFOV = camDiagFOV * cameraResWidth / hypotPixels;
-  camVertFOV = camDiagFOV * cameraResHeight / hypotPixels;
-
-  cam = SimPhotonCamera(name);
-  tgtList.clear();
-}
-
-void SimVisionSystem::AddSimVisionTarget(SimVisionTarget tgt) {
-  tgtList.push_back(tgt);
-}
-
-void SimVisionSystem::MoveCamera(frc::Transform2d newCameraToRobot,
-                                 units::meter_t newCamHeight) {
-  cameraToRobot = newCameraToRobot;
-  cameraHeightOffGround = newCamHeight;
-}
-
-void SimVisionSystem::ProcessFrame(frc::Pose2d robotPose) {
-  frc::Pose2d cameraPos = robotPose.TransformBy(cameraToRobot.Inverse());
-  std::vector<PhotonTrackedTarget> visibleTgtList = {};
-
-  for (auto&& tgt : tgtList) {
-    frc::Transform2d camToTargetTrans =
-        frc::Transform2d(cameraPos, tgt.targetPos);
-
-    units::meter_t distAlongGround = camToTargetTrans.Translation().Norm();
-    units::meter_t distVertical =
-        tgt.targetHeightAboveGround - cameraHeightOffGround;
-    units::meter_t distHypot =
-        units::math::hypot(distAlongGround, distVertical);
-
-    double area = tgt.tgtArea.value() / GetM2PerPx(distAlongGround);
-
-    // 2D yaw mode considers the target as a point, and should ignore target
-    // rotation.
-    // Photon reports it in the correct robot reference frame.
-    // IE: targets to the left of the image should report negative yaw.
-    units::degree_t yawAngle = -units::math::atan2(
-        camToTargetTrans.Translation().Y(), camToTargetTrans.Translation().X());
-    units::degree_t pitchAngle =
-        units::math::atan2(distVertical, distAlongGround);
-
-    auto translation = frc::Translation3d(camToTargetTrans.Translation().X(),
-                                          camToTargetTrans.Translation().Y(),
-                                          units::meter_t(0));  // TODO z height
-    auto rotation = frc::Rotation3d(units::radian_t(0), pitchAngle, -yawAngle);
-    frc::Transform3d camToTarget3d{translation, rotation};
-
-    if (CamCanSeeTarget(distHypot, yawAngle, pitchAngle, area)) {
-      PhotonTrackedTarget newTgt = PhotonTrackedTarget(
-          yawAngle.value(), pitchAngle.value(), area, 0.0, -1, camToTarget3d,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}});
-      visibleTgtList.push_back(newTgt);
-    }
-  }
-
-  units::second_t procDelay(0.0);  // Future - tie this to something meaningful
-  cam.SubmitProcessedFrame(procDelay,
-                           wpi::span<PhotonTrackedTarget>(visibleTgtList));
-}
-
-double SimVisionSystem::GetM2PerPx(units::meter_t dist) {
-  double heightMPerPx =
-      2 * dist.value() * units::math::tan(camVertFOV / 2) / cameraResHeight;
-  double widthMPerPx =
-      2 * dist.value() * units::math::tan(camHorizFOV / 2) / cameraResWidth;
-  return widthMPerPx * heightMPerPx;
-}
-
-bool SimVisionSystem::CamCanSeeTarget(units::meter_t distHypot,
-                                      units::degree_t yaw,
-                                      units::degree_t pitch, double area) {
-  bool inRange = (distHypot < maxLEDRange);
-  bool inHorizAngle = units::math::abs(yaw) < (camHorizFOV / 2);
-  bool inVertAngle = units::math::abs(pitch) < (camVertFOV / 2);
-  bool targetBigEnough = area > minTargetArea;
-  return (inRange && inHorizAngle && inVertAngle && targetBigEnough);
-}
-
-}  // namespace photonlib
--- a/photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp
@@ -1,40 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2022 PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include "photonlib/SimVisionTarget.h"
-
-namespace photonlib {
-
-SimVisionTarget::SimVisionTarget(frc::Pose2d& targetPos,
-                                 units::meter_t targetHeightAboveGround,
-                                 units::meter_t targetWidth,
-                                 units::meter_t targetHeight)
-    : targetPos(targetPos),
-      targetHeightAboveGround(targetHeightAboveGround),
-      targetWidth(targetWidth),
-      targetHeight(targetHeight) {
-  tgtArea = targetWidth * targetHeight;
-}
-
-}  // namespace photonlib
--- a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h
+++ b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h
@@ -79,6 +79,22 @@ class PhotonPipelineResult {
   */
  units::second_t GetLatency() const { return latency; }

+  /**
+   * Returns the estimated time the frame was taken,
+   * This is much more accurate than using GetLatency()
+   * @return The timestamp in seconds or -1 if this result was not initiated
+   * with a timestamp.
+   */
+  units::second_t GetTimestamp() const { return timestamp; }
+
+  /**
+   * Sets the timestamp in seconds
+   * @param timestamp The timestamp in seconds
+   */
+  void SetTimestamp(const units::second_t timestamp) {
+    this->timestamp = timestamp;
+  }
+
  /**
   * Returns whether the pipeline has targets.
   * @return Whether the pipeline has targets.
@@ -101,6 +117,7 @@ class PhotonPipelineResult {

 private:
  units::second_t latency = 0_s;
+  units::second_t timestamp = -1_s;
  wpi::SmallVector<PhotonTrackedTarget, 10> targets;
  inline static bool HAS_WARNED = false;
 };
--- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h
+++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h
@@ -1,75 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2022 PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#pragma once
-
-#include <memory>
-#include <string>
-
-#include <units/time.h>
-#include <wpi/SmallVector.h>
-#include <wpi/span.h>
-
-#include "photonlib/Packet.h"
-#include "photonlib/PhotonCamera.h"
-
-namespace photonlib {
-
-/**
- * Represents a camera that is connected to PhotonVision.ß
- */
-class SimPhotonCamera : public PhotonCamera {
- public:
-  /**
-   * Constructs a Simulated PhotonCamera from a root table.
-   *
-   * @param instance The NetworkTableInstance to pull data from. This can be a
-   * custom instance in simulation, but should *usually* be the default
-   * NTInstance from {@link NetworkTableInstance::getDefault}
-   * @param cameraName The name of the camera, as seen in the UI.
-   */
-  explicit SimPhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
-                           const std::string& cameraName);
-
-  /**
-   * Constructs a Simulated PhotonCamera from the name of the camera.
-   *
-   * @param cameraName The nickname of the camera (found in the PhotonVision
-   *                   UI).
-   */
-  explicit SimPhotonCamera(const std::string& cameraName);
-
-  /**
-   * Simulate one processed frame of vision data, putting one result to NT.
-   * @param latency Latency of frame processing
-   * @param tgtList Set of targets detected
-   */
-  void SubmitProcessedFrame(units::second_t latency,
-                            wpi::span<const PhotonTrackedTarget> tgtList);
-
- private:
-  mutable Packet simPacket;
-};
-
-}  // namespace photonlib
--- a/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h
+++ b/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h
@@ -1,77 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2022 PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <frc/geometry/Translation2d.h>
-#include <units/angle.h>
-#include <units/area.h>
-#include <units/length.h>
-#include <units/time.h>
-#include <wpi/SmallVector.h>
-
-#include "photonlib/SimPhotonCamera.h"
-#include "photonlib/SimVisionTarget.h"
-
-namespace photonlib {
-
-/**
- * Represents a camera that is connected to PhotonVision.
- */
-class SimVisionSystem {
- public:
-  explicit SimVisionSystem(const std::string& name, units::degree_t camDiagFOV,
-                           frc::Transform2d cameraToRobot,
-                           units::meter_t cameraHeightOffGround,
-                           units::meter_t maxLEDRange, int cameraResWidth,
-                           int cameraResHeight, double minTargetArea);
-
-  void AddSimVisionTarget(SimVisionTarget tgt);
-  void MoveCamera(frc::Transform2d newcameraToRobot,
-                  units::meter_t newCamHeight);
-  void ProcessFrame(frc::Pose2d robotPose);
-
- private:
-  frc::Transform2d cameraToRobot;
-  units::meter_t cameraHeightOffGround;
-  units::meter_t maxLEDRange;
-  int cameraResWidth;
-  int cameraResHeight;
-  double minTargetArea;
-  units::degree_t camHorizFOV;
-  units::degree_t camVertFOV;
-  std::vector<SimVisionTarget> tgtList = {};
-
-  double GetM2PerPx(units::meter_t dist);
-  bool CamCanSeeTarget(units::meter_t distHypot, units::degree_t yaw,
-                       units::degree_t pitch, double area);
-
- public:
-  SimPhotonCamera cam = photonlib::SimPhotonCamera("Default");
-};
-
-}  // namespace photonlib
--- a/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h
+++ b/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h
@@ -1,51 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2022 PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#pragma once
-
-#include <frc/geometry/Pose2d.h>
-#include <units/area.h>
-#include <units/length.h>
-
-namespace photonlib {
-
-/**
- * Represents a target on the field which the vision processing system could
- * detect.
- */
-class SimVisionTarget {
- public:
-  explicit SimVisionTarget(frc::Pose2d& targetPos,
-                           units::meter_t targetHeightAboveGround,
-                           units::meter_t targetWidth,
-                           units::meter_t targetHeight);
-
-  frc::Pose2d targetPos;
-  units::meter_t targetHeightAboveGround;
-  units::meter_t targetWidth;
-  units::meter_t targetHeight;
-  units::square_meter_t tgtArea;
-};
-
-}  // namespace photonlib
--- a/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java
+++ b/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java
@@ -29,13 +29,19 @@ import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;

 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.Transform2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import java.util.List;
 import java.util.stream.Stream;
+import org.junit.jupiter.api.AfterAll;
 import org.junit.jupiter.api.Assertions;
+import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
 import org.junit.jupiter.params.ParameterizedTest;
 import org.junit.jupiter.params.provider.Arguments;
@@ -49,18 +55,30 @@ class SimVisionSystemTest {
        Assertions.assertDoesNotThrow(
                () -> {
                    var sysUnderTest =
-                            new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 320, 240, 0);
-                    sysUnderTest.addSimVisionTarget(new SimVisionTarget(new Pose2d(), 0.0, 1.0, 1.0));
+                            new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 320, 240, 0);
+                    sysUnderTest.addSimVisionTarget(new SimVisionTarget(new Pose3d(), 1.0, 1.0, 42));
                    for (int loopIdx = 0; loopIdx < 100; loopIdx++) {
                        sysUnderTest.processFrame(new Pose2d());
                    }
                });
    }

+    @BeforeAll
+    public static void setUp() {
+        // NT live for debug purposes
+        NetworkTableInstance.getDefault().startServer();
+
+        // No version check for testing
+        PhotonCamera.setVersionCheckEnabled(false);
+    }
+
+    @AfterAll
+    public static void shutDown() {}
+
    // @ParameterizedTest
    // @ValueSource(doubles = {5, 10, 15, 20, 25, 30})
    // public void testDistanceAligned(double dist) {
-    //     final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
+    //     final var targetPose = new Pose2d(new Translation2d(15.98, 0), new Rotation2d());
    //     var sysUnderTest =
    //             new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 320, 240, 0);
    //     sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 1.0, 1.0));
@@ -76,10 +94,10 @@ class SimVisionSystemTest {

    @Test
    public void testVisibilityCupidShuffle() {
-        final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
-        var sysUnderTest =
-                new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0);
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3.0));
+        final var targetPose =
+                new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI));
+        var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0);
+        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3));

        // To the right, to the right
        var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70));
@@ -117,37 +135,40 @@ class SimVisionSystemTest {
        assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());

        // now walk it by yourself
-        sysUnderTest.moveCamera(
-                new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180)), 0, 1.0);
+        sysUnderTest.moveCamera(new Transform3d(new Translation3d(), new Rotation3d(0, 0, Math.PI)));
        sysUnderTest.processFrame(robotPose);
        assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
    }

    @Test
    public void testNotVisibleVert1() {
-        final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
-        var sysUnderTest =
-                new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0);
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3.0));
+        final var targetPose =
+                new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI));
+        var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0);
+        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3));

-        PhotonCamera.setVersionCheckEnabled(false);
        var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5));
        sysUnderTest.processFrame(robotPose);
        assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());

-        sysUnderTest.moveCamera(new Transform2d(), 5000, 1.0); // vooop selfie stick
+        sysUnderTest.moveCamera(
+                new Transform3d(
+                        new Translation3d(0, 0, 5000), new Rotation3d(0, 0, Math.PI))); // vooop selfie stick
        sysUnderTest.processFrame(robotPose);
        assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
    }

    @Test
    public void testNotVisibleVert2() {
-        final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
+        final var targetPose =
+                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, 45.0, new Transform2d(), 1, 99999, 1234, 1234, 0);
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 0.5));
+                new SimVisionSystem("Test", 80.0, robotToCamera.inverse(), 99999, 1234, 1234, 0);
+        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 1736));

-        var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(5));
+        var robotPose = new Pose2d(new Translation2d(14.98, 0), Rotation2d.fromDegrees(5));
        sysUnderTest.processFrame(robotPose);
        assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());

@@ -159,12 +180,12 @@ class SimVisionSystemTest {

    @Test
    public void testNotVisibleTgtSize() {
-        final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
-        var sysUnderTest =
-                new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 20.0);
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 0.1));
+        final var targetPose =
+                new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI));
+        var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 20.0);
+        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.1, 0.025, 24));

-        var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(5));
+        var robotPose = new Pose2d(new Translation2d(12, 0), Rotation2d.fromDegrees(5));
        sysUnderTest.processFrame(robotPose);
        assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());

@@ -175,12 +196,12 @@ class SimVisionSystemTest {

    @Test
    public void testNotVisibleTooFarForLEDs() {
-        final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
-        var sysUnderTest =
-                new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 10, 640, 480, 1.0);
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 0.1));
+        final var targetPose =
+                new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI));
+        var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 10, 640, 480, 1.0);
+        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 78));

-        var robotPose = new Pose2d(new Translation2d(28, 0), Rotation2d.fromDegrees(5));
+        var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(5));
        sysUnderTest.processFrame(robotPose);
        assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());

@@ -192,12 +213,12 @@ class SimVisionSystemTest {
    @ParameterizedTest
    @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23})
    public void testYawAngles(double testYaw) {
-        final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 4));
-        var sysUnderTest =
-                new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0);
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 0.5, 0.5));
+        final var targetPose =
+                new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, 3 * Math.PI / 4));
+        var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0);
+        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 3));

-        var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(testYaw));
+        var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(-1.0 * testYaw));
        sysUnderTest.processFrame(robotPose);
        var res = sysUnderTest.cam.getLatestResult();
        assertTrue(res.hasTargets());
@@ -208,45 +229,48 @@ class SimVisionSystemTest {
    @ParameterizedTest
    @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999})
    public void testCameraPitch(double testPitch) {
-        final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 4));
-        final var robotPose = new Pose2d(new Translation2d(30, 0), new Rotation2d(0));
-        var sysUnderTest =
-                new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 0.0, 99999, 640, 480, 0);
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 0.5, 0.5));
+        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));

-        sysUnderTest.moveCamera(new Transform2d(), 0.0, testPitch);
+        // Here, passing in a positive testPitch points the camera downward (since moveCamera takes the
+        // camera->robot transform)
+        sysUnderTest.moveCamera(
+                new Transform3d(
+                        new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0)));
        sysUnderTest.processFrame(robotPose);
        var res = sysUnderTest.cam.getLatestResult();
        assertTrue(res.hasTargets());
        var tgt = res.getBestTarget();
-        // If the camera is pitched down by 10 degrees, the target should appear
-        // in the upper part of the image (ie, pitch positive). Therefor,
-        // pass/fail involves -1.0.
-        assertEquals(tgt.getPitch(), -testPitch, 0.0001);
+
+        // 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);
    }

    private static Stream<Arguments> distCalCParamProvider() {
        // Arbitrary and fairly random assortment of distances, camera pitches, and heights
        return Stream.of(
-                Arguments.of(5, 35, 0),
-                Arguments.of(6, 35, 1),
-                Arguments.of(10, 35, 0),
-                Arguments.of(15, 35, 2),
-                Arguments.of(19.95, 35, 0),
-                Arguments.of(20, 35, 0),
+                Arguments.of(5, 15.98, 0),
+                Arguments.of(6, 15.98, 1),
+                Arguments.of(10, 15.98, 0),
+                Arguments.of(15, 15.98, 2),
+                Arguments.of(19.95, 15.98, 0),
+                Arguments.of(20, 15.98, 0),
                Arguments.of(5, 42, 1),
                Arguments.of(6, 42, 0),
                Arguments.of(10, 42, 2),
                Arguments.of(15, 42, 0.5),
-                Arguments.of(19.42, 35, 0),
+                Arguments.of(19.42, 15.98, 0),
                Arguments.of(20, 42, 0),
                Arguments.of(5, 55, 2),
                Arguments.of(6, 55, 0),
                Arguments.of(10, 54, 2.2),
                Arguments.of(15, 53, 0),
-                Arguments.of(19.52, 35, 1.1),
-                Arguments.of(20, 51, 2.87),
-                Arguments.of(20, 55, 3));
+                Arguments.of(19.52, 15.98, 1.1));
    }

    @ParameterizedTest
@@ -254,20 +278,25 @@ class SimVisionSystemTest {
    public void testDistanceCalc(double testDist, double testPitch, double testHeight) {
        // Assume dist along ground and tgt height the same. Iterate over other parameters.

-        final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 42));
-        final var robotPose = new Pose2d(new Translation2d(35 - testDist, 0), new Rotation2d(0));
+        final var targetPose =
+                new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI * 0.98));
+        final var robotPose =
+                new Pose3d(new Translation3d(15.98 - Units.feetToMeters(testDist), 0, 0), new Rotation3d());
+        final var robotToCamera =
+                new Transform3d(
+                        new Translation3d(0, 0, Units.feetToMeters(testHeight)),
+                        new Rotation3d(0, Units.degreesToRadians(testPitch), 0));
+
        var sysUnderTest =
                new SimVisionSystem(
                        "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!",
                        160.0,
-                        testPitch,
-                        new Transform2d(),
-                        testHeight,
+                        robotToCamera.inverse(),
                        99999,
                        640,
                        480,
                        0);
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, testDist, 0.5, 0.5));
+        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 0));

        sysUnderTest.processFrame(robotPose);
        var res = sysUnderTest.cam.getLatestResult();
@@ -276,34 +305,102 @@ class SimVisionSystemTest {
        assertEquals(tgt.getYaw(), 0.0, 0.0001);
        double distMeas =
                PhotonUtils.calculateDistanceToTargetMeters(
-                        testHeight,
-                        testDist,
+                        robotToCamera.getZ(),
+                        targetPose.getZ(),
                        Units.degreesToRadians(testPitch),
                        Units.degreesToRadians(tgt.getPitch()));
-        assertEquals(distMeas, testDist, 0.001);
+        assertEquals(Units.feetToMeters(testDist), distMeas, 0.001);
    }

    @Test
    public void testMultipleTargets() {
-        final var targetPoseL = new Pose2d(new Translation2d(35, 2), new Rotation2d());
-        final var targetPoseC = new Pose2d(new Translation2d(35, 0), new Rotation2d());
-        final var targetPoseR = new Pose2d(new Translation2d(35, -2), new Rotation2d());
-        var sysUnderTest =
-                new SimVisionSystem("Test", 160.0, 0.0, new Transform2d(), 5.0, 99999, 640, 480, 20.0);
+        final var targetPoseL =
+                new Pose3d(new Translation3d(15.98, 2, 0), new Rotation3d(0, 0, Math.PI));
+        final var targetPoseC =
+                new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, Math.PI));
+        final var targetPoseR =
+                new Pose3d(new Translation3d(15.98, -2, 0), new Rotation3d(0, 0, Math.PI));
+        var sysUnderTest = new SimVisionSystem("Test", 160.0, new Transform3d(), 99999, 640, 480, 20.0);

-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 0.0, 0.25, 0.1));
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 1.0, 0.25, 0.1));
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 2.0, 0.25, 0.1));
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 3.0, 0.25, 0.1));
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 4.0, 0.25, 0.1));
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 5.0, 0.25, 0.1));
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 6.0, 0.25, 0.1));
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 7.0, 0.25, 0.1));
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 8.0, 0.25, 0.1));
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 9.0, 0.25, 0.1));
-        sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 10.0, 0.25, 0.1));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseL.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        1));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseC.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        2));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseR.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        3));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseL.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        4));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseC.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        5));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseR.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        6));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseL.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        7));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseC.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        8));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseL.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        9));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseR.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        10));
+        sysUnderTest.addSimVisionTarget(
+                new SimVisionTarget(
+                        targetPoseL.transformBy(
+                                new Transform3d(new Translation3d(0, 0, 0.25), new Rotation3d())),
+                        0.25,
+                        0.25,
+                        11));

-        var robotPose = new Pose2d(new Translation2d(30, 0), Rotation2d.fromDegrees(5));
+        var robotPose = new Pose2d(new Translation2d(6.0, 0), Rotation2d.fromDegrees(0.25));
        sysUnderTest.processFrame(robotPose);
        var res = sysUnderTest.cam.getLatestResult();
        assertTrue(res.hasTargets());
--- a/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp
+++ b/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp
@@ -1,416 +0,0 @@
-/*
- * 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.
- */
-
-// So for now, with the new apriltag stuff, this is totally broken
-// For now, commented out
-
-/*
-#include <networktables/NetworkTable.h>
-#include <networktables/NetworkTableEntry.h>
-#include <networktables/NetworkTableInstance.h>
-#include <units/angle.h>
-#include <units/length.h>
-
-#include "gtest/gtest.h"
-#include "photonlib/PhotonCamera.h"
-#include "photonlib/PhotonUtils.h"
-#include "photonlib/SimVisionSystem.h"
-
-TEST(SimVisionSystemTest, Empty) {
-  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg,
-                                          frc::Transform2d(), 1.0_m, 99999.0_m,
-                                          320, 240, 0.0);
-
-  for (int loopIdx = 0; loopIdx < 100; loopIdx++) {
-    sysUnderTest.ProcessFrame(frc::Pose2d());
-  }
-}
-
-class SimVisionSystemDistParamTest : public testing::TestWithParam<double> {};
-INSTANTIATE_TEST_SUITE_P(SimVisionSystemDistParamTests,
-                         SimVisionSystemDistParamTest,
-                         testing::Values(5, 10, 15, 20, 25, 30));
-
-TEST_P(SimVisionSystemDistParamTest, DistanceAligned) {
-  double dist = GetParam();
-
-  auto targetPose =
-      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
-
-  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg,
-                                          frc::Transform2d(), 1.0_m, 99999.0_m,
-                                          320, 240, 0.0);
-
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPose, 0.0_m, 1.0_m, 1.0_m));
-
-  auto robotPose = frc::Pose2d(
-      frc::Translation2d(units::meter_t(35.0 - dist), 0_m), frc::Rotation2d());
-
-  sysUnderTest.ProcessFrame(robotPose);
-  auto result = sysUnderTest.cam.GetLatestResult();
-  ASSERT_TRUE(result.HasTargets());
-  std::cout << "Best target pitch " <<
-result.GetBestTarget().GetCameraToTarget().Translation().X().value();
-
-  ASSERT_EQ(result.GetBestTarget()
-                .GetCameraToTarget()
-                .Translation()
-                .Norm()
-                .value(),
-            dist);
-}
-
-TEST(SimVisionSystemTest, VisibilityCupidShuffle) {
-  auto targetPose =
-      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
-
-  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg,
-                                          frc::Transform2d(), 1.0_m, 99999.0_m,
-                                          320, 240, 0.0);
-
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPose, 0.0_m, 3.0_m, 3.0_m));
-
-  // To the right, to the right
-  auto robotPose =
-      frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(-70.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  auto result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_FALSE(result.HasTargets());
-
-  // To the right, to the right
-  robotPose =
-      frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(-95.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_FALSE(result.HasTargets());
-
-  // To the left, to the left
-  robotPose =
-      frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(90.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_FALSE(result.HasTargets());
-
-  // To the left, to the left
-  robotPose =
-      frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(65.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_FALSE(result.HasTargets());
-
-  // now kick, now kick
-  robotPose =
-      frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_TRUE(result.HasTargets());
-
-  // now kick, now kick
-  robotPose =
-      frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), frc::Rotation2d(-5.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_TRUE(result.HasTargets());
-
-  // now walk it by yourself
-  robotPose = frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m),
-                          frc::Rotation2d(-179.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_FALSE(result.HasTargets());
-
-  // now walk it by yourself
-  sysUnderTest.MoveCamera(
-      frc::Transform2d(frc::Translation2d(), frc::Rotation2d(180_deg)), 0.0_m);
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_TRUE(result.HasTargets());
-}
-
-TEST(SimVisionSystemTest, NotVisibleVert1) {
-  auto targetPose =
-      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
-
-  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg,
-                                          frc::Transform2d(), 1.0_m, 99999.0_m,
-                                          640, 480, 0.0);
-
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPose, 1.0_m, 3.0_m, 2.0_m));
-
-  auto robotPose =
-      frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
-
-  sysUnderTest.ProcessFrame(robotPose);
-  auto result = sysUnderTest.cam.GetLatestResult();
-  ASSERT_TRUE(result.HasTargets());
-
-  sysUnderTest.MoveCamera(
-      frc::Transform2d(frc::Translation2d(), frc::Rotation2d(0_deg)), 5000.0_m);
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_FALSE(result.HasTargets());
-}
-
-TEST(SimVisionSystemTest, NotVisibleVert2) {
-  auto targetPose =
-      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
-
-  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg,
-                                          frc::Transform2d(), 1.0_m, 99999.0_m,
-                                          1234, 1234, 0.5);
-
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPose, 3.0_m, 0.5_m, 0.5_m));
-
-  auto robotPose =
-      frc::Pose2d(frc::Translation2d(32.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
-
-  sysUnderTest.ProcessFrame(robotPose);
-  auto result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_TRUE(result.HasTargets());
-
-  robotPose =
-      frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_FALSE(result.HasTargets());
-}
-
-TEST(SimVisionSystemTest, NotVisibleTgtSize) {
-  auto targetPose =
-      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
-
-  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg,
-                                          frc::Transform2d(), 1.0_m, 99999.0_m,
-                                          640, 480, 20.0);
-
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPose, 1.10_m, 0.25_m, 0.1_m));
-
-  auto robotPose =
-      frc::Pose2d(frc::Translation2d(32.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  auto result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_TRUE(result.HasTargets());
-
-  robotPose =
-      frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_FALSE(result.HasTargets());
-}
-
-TEST(SimVisionSystemTest, NotVisibleTooFarForLEDs) {
-  auto targetPose =
-      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
-
-  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg,
-                                          frc::Transform2d(), 1.0_m, 10.0_m,
-                                          640, 480, 1.0);
-
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPose, 1.10_m, 0.25_m, 0.1_m));
-
-  auto robotPose =
-      frc::Pose2d(frc::Translation2d(28.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  auto result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_TRUE(result.HasTargets());
-
-  robotPose =
-      frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
-  sysUnderTest.ProcessFrame(robotPose);
-  result = sysUnderTest.cam.GetLatestResult();
-  EXPECT_FALSE(result.HasTargets());
-}
-
-class SimVisionSystemYawParamTest : public testing::TestWithParam<double> {};
-INSTANTIATE_TEST_SUITE_P(SimVisionSystemYawParamTests,
-                         SimVisionSystemYawParamTest,
-                         testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23));
-TEST_P(SimVisionSystemYawParamTest, YawAngles) {
-  double testYaw = GetParam();  // Nope, Chuck testYaw
-  auto targetPose =
-      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d(45_deg));
-
-  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg,
-                                          frc::Transform2d(), 1.0_m, 99999.0_m,
-                                          640, 480, 0.0);
-
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPose, 0.0_m, 0.5_m, 0.5_m));
-
-  auto robotPose = frc::Pose2d(frc::Translation2d(32_m, 0.0_m),
-                               frc::Rotation2d(units::degree_t(testYaw)));
-
-  sysUnderTest.ProcessFrame(robotPose);
-  auto result = sysUnderTest.cam.GetLatestResult();
-  ASSERT_TRUE(result.HasTargets());
-  auto tgt = result.GetBestTarget();
-  EXPECT_DOUBLE_EQ(tgt.GetYaw(), testYaw);
-}
-
-// class SimVisionSystemCameraPitchParamTest
-//     : public testing::TestWithParam<double> {};
-// INSTANTIATE_TEST_SUITE_P(SimVisionSystemCameraPitchParamTests,
-//                          SimVisionSystemCameraPitchParamTest,
-//                          testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23,
-//                                          20.21, -19.999));
-// TEST_P(SimVisionSystemCameraPitchParamTest, CameraPitch) {
-//   double testPitch = GetParam();
-//   auto targetPose =
-//       frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d(45_deg));
-
-//   auto robotPose =
-//       frc::Pose2d(frc::Translation2d(30_m, 0.0_m), frc::Rotation2d(0.0_deg));
-
-//   photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg,
-//                                           frc::Transform2d(), 1.0_m,
-99999.0_m,
-//                                           640, 480, 0.0);
-
-//   sysUnderTest.AddSimVisionTarget(
-//       photonlib::SimVisionTarget(targetPose, 0.0_m, 0.5_m, 0.5_m));
-
-//   sysUnderTest.MoveCamera(
-//       frc::Transform2d(frc::Translation2d(), frc::Rotation2d()), 0.0_m));
-
-//   photonlib::PhotonCamera::SetVersionCheckEnabled(false);
-//   sysUnderTest.ProcessFrame(robotPose);
-//   auto result = sysUnderTest.cam.GetLatestResult();
-//   ASSERT_TRUE(result.HasTargets());
-//   auto tgt = result.GetBestTarget();
-//   // If the camera is pitched down by 10 degrees, the target should appear
-//   // in the upper part of the image (ie, pitch positive). Therefor,
-//   // pass/fail involves -1.0.
-//   EXPECT_DOUBLE_EQ(tgt.GetPitch(), -testPitch);
-// }
-
-class SimVisionSystemDistCalcParamTest
-    : public testing::TestWithParam<std::tuple<double, double, double>> {};
-INSTANTIATE_TEST_SUITE_P(
-    SimVisionSystemDistCalcParamTests, SimVisionSystemDistCalcParamTest,
-    testing::Values(std::tuple<double, double, double>(5, 35, 0),
-                    std::tuple<double, double, double>(6, 35, 1),
-                    std::tuple<double, double, double>(10, 35, 0),
-                    std::tuple<double, double, double>(15, 35, 2),
-                    std::tuple<double, double, double>(19.95, 35, 0),
-                    std::tuple<double, double, double>(20, 35, 0),
-                    std::tuple<double, double, double>(5, 42, 1),
-                    std::tuple<double, double, double>(6, 42, 0),
-                    std::tuple<double, double, double>(10, 42, 2),
-                    std::tuple<double, double, double>(15, 42, 0.5),
-                    std::tuple<double, double, double>(19.42, 35, 0),
-                    std::tuple<double, double, double>(20, 42, 0),
-                    std::tuple<double, double, double>(5, 55, 2),
-                    std::tuple<double, double, double>(6, 55, 0),
-                    std::tuple<double, double, double>(10, 54, 2.2),
-                    std::tuple<double, double, double>(15, 53, 0),
-                    std::tuple<double, double, double>(19.52, 35, 1.1),
-                    std::tuple<double, double, double>(20, 51, 2.87),
-                    std::tuple<double, double, double>(20, 55, 3)));
-// TEST_P(SimVisionSystemDistCalcParamTest, DistanceCalc) {
-//   std::tuple<double, double, double> testArgs = GetParam();
-//   double testDist = std::get<0>(testArgs);
-//   double testPitch = std::get<1>(testArgs);
-//   double testHeight = std::get<2>(testArgs);
-
-//   auto targetPose = frc::Pose2d(frc::Translation2d(35_m, 0_m),
-//                                 frc::Rotation2d(units::radian_t(3.14159 /
-//                                                                      42)));
-
-//   auto robotPose =
-//       frc::Pose2d(frc::Translation2d(units::meter_t(35 - testDist), 0.0_m),
-//                   frc::Rotation2d(0.0_deg));
-
-//   photonlib::SimVisionSystem sysUnderTest(
-// "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho"
-//       "wsyourdaygoingihopegoodhaveagreatrestofyourlife",
-//       160.0_deg, units::degree_t(testPitch), frc::Transform2d(),
-//       units::meter_t(testHeight), 99999.0_m, 640, 480, 0.0);
-
-//   sysUnderTest.AddSimVisionTarget(photonlib::SimVisionTarget(
-//       targetPose, units::meter_t(testDist), 0.5_m, 0.5_m));
-
-//   sysUnderTest.ProcessFrame(robotPose);
-//   auto result = sysUnderTest.cam.GetLatestResult();
-//   ASSERT_TRUE(result.HasTargets());
-//   auto tgt = result.GetBestTarget();
-//   EXPECT_DOUBLE_EQ(tgt.GetYaw(), 0.0);
-//   units::meter_t distMeas =
-//   photonlib::PhotonUtils::CalculateDistanceToTarget(
-//       units::meter_t(testHeight), units::meter_t(testDist),
-//       units::degree_t(testPitch), units::degree_t(tgt.GetPitch()));
-//   EXPECT_DOUBLE_EQ(distMeas.value(), testDist);
-// }
-
-TEST(SimVisionSystemTest, MultipleTargets) {
-  auto targetPoseL =
-      frc::Pose2d(frc::Translation2d(35_m, 2_m), frc::Rotation2d());
-  auto targetPoseC =
-      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
-  auto targetPoseR =
-      frc::Pose2d(frc::Translation2d(35_m, -2_m), frc::Rotation2d());
-
-  photonlib::SimVisionSystem sysUnderTest("test", 160.0_deg,
-                                          frc::Transform2d(), 5.0_m, 99999.0_m,
-                                          640, 480, 20.0);
-
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseL, 0.0_m, 0.25_m, 0.1_m));
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseC, 1.0_m, 0.25_m, 0.1_m));
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseR, 2.0_m, 0.25_m, 0.1_m));
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseL, 3.0_m, 0.25_m, 0.1_m));
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseC, 4.0_m, 0.25_m, 0.1_m));
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseR, 5.0_m, 0.25_m, 0.1_m));
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseL, 6.0_m, 0.25_m, 0.1_m));
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseC, 7.0_m, 0.25_m, 0.1_m));
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseL, 8.0_m, 0.25_m, 0.1_m));
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseR, 9.0_m, 0.25_m, 0.1_m));
-  sysUnderTest.AddSimVisionTarget(
-      photonlib::SimVisionTarget(targetPoseL, 10.0_m, 0.25_m, 0.1_m));
-
-  auto robotPose =
-      frc::Pose2d(frc::Translation2d(30_m, 0.0_m), frc::Rotation2d(5.0_deg));
-
-  sysUnderTest.ProcessFrame(robotPose);
-  auto result = sysUnderTest.cam.GetLatestResult();
-  ASSERT_TRUE(result.HasTargets());
-
-  auto tgtList = result.GetTargets();
-  EXPECT_EQ(11ul, tgtList.size());
-}
-*/
--- a/photon-server/src/main/resources/web/docs/index.html
+++ b/photon-server/src/main/resources/web/docs/index.html
@@ -1 +0,0 @@
-<p>Docs have not been copied!</p>
--- a/photon-server/src/main/resources/web/index.html
+++ b/photon-server/src/main/resources/web/index.html
@@ -1 +1 @@
-<!DOCTYPE html><html lang=en><head><meta charset=utf-8><meta http-equiv=X-UA-Compatible content="IE=edge"><meta name=viewport content="width=device-width,initial-scale=1"><link rel=icon href=/favicon.png><title>PhotonVision</title><link href=/js/chunk-2d216214.4302abb0.js rel=prefetch><link href=/js/chunk-2d216257.0de4b8cc.js rel=prefetch><link href=/js/chunk-ad0384ec.7dafafc8.js rel=prefetch><link href=/css/app.3dcf0da8.css rel=preload as=style><link href=/css/chunk-vendors.7e4af884.css rel=preload as=style><link href=/js/app.4e0aa52b.js rel=preload as=script><link href=/js/chunk-vendors.58a768b6.js rel=preload as=script><link href=/css/chunk-vendors.7e4af884.css rel=stylesheet><link href=/css/app.3dcf0da8.css rel=stylesheet></head><body><noscript><strong>We're sorry but PhotonVision doesn't work properly without JavaScript enabled. Please enable it to continue.</strong></noscript><div id=app></div><script src=/js/chunk-vendors.58a768b6.js></script><script src=/js/app.4e0aa52b.js></script></body></html>
+<p>UI has not been copied!</p>
--- a/photon-targeting/build.gradle
+++ b/photon-targeting/build.gradle
@@ -1,5 +1,7 @@
 apply plugin: "java"

+apply from: "${rootDir}/shared/common.gradle"
+
 sourceCompatibility = 11
 targetCompatibility = 11

--- a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
+++ b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
@@ -17,6 +17,10 @@

 package edu.wpi.first.math.geometry;

+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -27,6 +31,8 @@ import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;

 /** Represents a 3D pose containing translational and rotational elements. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Pose3d implements Interpolatable<Pose3d> {
    private final Translation3d m_translation;
    private final Rotation3d m_rotation;
@@ -43,11 +49,13 @@ public class Pose3d implements Interpolatable<Pose3d> {
     * @param translation The translational component of the pose.
     * @param rotation The rotational component of the pose.
     */
-    public Pose3d(Translation3d translation, Rotation3d rotation) {
+    @JsonCreator
+    public Pose3d(
+            @JsonProperty(required = true, value = "translation") Translation3d translation,
+            @JsonProperty(required = true, value = "rotation") Rotation3d rotation) {
        m_translation = translation;
        m_rotation = rotation;
    }
-
    /**
     * Constructs a pose with x, y, and z translations instead of a separate Translation3d.
     *
@@ -91,6 +99,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
     *
     * @return The translational component of the pose.
     */
+    @JsonProperty
    public Translation3d getTranslation() {
        return m_translation;
    }
@@ -127,6 +136,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
     *
     * @return The rotational component of the pose.
     */
+    @JsonProperty
    public Rotation3d getRotation() {
        return m_rotation;
    }
--- a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
+++ b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
@@ -17,22 +17,25 @@

 package edu.wpi.first.math.geometry;

-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.numbers.N1;
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.numbers.N3;
-import java.util.Arrays;
 import java.util.Objects;

+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Quaternion {
    private final double m_r;
-    private final Matrix<N3, N1> m_v;
+    private final Vector<N3> m_v;

    /** Constructs a quaternion with a default angle of 0 degrees. */
    public Quaternion() {
        m_r = 1.0;
-        m_v = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0, 0.0, 0.0);
+        m_v = VecBuilder.fill(0.0, 0.0, 0.0);
    }

    /**
@@ -43,9 +46,14 @@ public class Quaternion {
     * @param y Y component of the quaternion.
     * @param z Z component of the quaternion.
     */
-    public Quaternion(double w, double x, double y, double z) {
+    @JsonCreator
+    public Quaternion(
+            @JsonProperty(required = true, value = "W") double w,
+            @JsonProperty(required = true, value = "X") double x,
+            @JsonProperty(required = true, value = "Y") double y,
+            @JsonProperty(required = true, value = "Z") double z) {
        m_r = w;
-        m_v = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(x, y, z);
+        m_v = VecBuilder.fill(x, y, z);
    }

    /**
@@ -61,21 +69,23 @@ public class Quaternion {
        final var r2 = other.m_r;
        final var v2 = other.m_v;

-        final var v1x = v1.get(0, 0);
-        final var v1y = v1.get(1, 0);
-        final var v1z = v1.get(2, 0);
-
-        final var v2x = v2.get(0, 0);
-        final var v2y = v2.get(1, 0);
-        final var v2z = v2.get(2, 0);
-
+        // v₁ x v₂
        var cross =
-                new MatBuilder<>(Nat.N3(), Nat.N1())
-                        .fill(v1y * v2z - v2y * v1z, v2x * v1z - v1x * v2z, v1x * v2y - v2x * v1y);
-        double dot = v1x * v2x + v1y * v2y + v1z * v2z;
+                VecBuilder.fill(
+                        v1.get(1, 0) * v2.get(2, 0) - v2.get(1, 0) * v1.get(2, 0),
+                        v2.get(0, 0) * v1.get(2, 0) - v1.get(0, 0) * v2.get(2, 0),
+                        v1.get(0, 0) * v2.get(1, 0) - v2.get(0, 0) * v1.get(1, 0));

+        // v = r₁v₂ + r₂v₁ + v₁ x v₂
        final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
-        return new Quaternion(r1 * r2 - dot, v.get(0, 0), v.get(1, 0), v.get(2, 0));
+
+        return new Quaternion(r1 * r2 - v1.dot(v2), v.get(0, 0), v.get(1, 0), v.get(2, 0));
+    }
+
+    @Override
+    public String toString() {
+        return String.format(
+                "Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0));
    }

    /**
@@ -89,20 +99,7 @@ public class Quaternion {
        if (obj instanceof Quaternion) {
            var other = (Quaternion) obj;

-            final var r1 = m_r;
-            final var v1 = m_v;
-            final var r2 = other.m_r;
-            final var v2 = other.m_v;
-
-            final var v1x = v1.get(0, 0);
-            final var v1y = v1.get(1, 0);
-            final var v1z = v1.get(2, 0);
-
-            final var v2x = v2.get(0, 0);
-            final var v2y = v2.get(1, 0);
-            final var v2z = v2.get(2, 0);
-
-            return Math.abs(r1 * r2 + v1x * v2x + v1y * v2y + v1z * v2z) > 1.0 - 1E-9;
+            return Math.abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
        }
        return false;
    }
@@ -140,6 +137,7 @@ public class Quaternion {
     *
     * @return W component of the quaternion.
     */
+    @JsonProperty(value = "W")
    public double getW() {
        return m_r;
    }
@@ -149,6 +147,7 @@ public class Quaternion {
     *
     * @return X component of the quaternion.
     */
+    @JsonProperty(value = "X")
    public double getX() {
        return m_v.get(0, 0);
    }
@@ -158,6 +157,7 @@ public class Quaternion {
     *
     * @return Y component of the quaternion.
     */
+    @JsonProperty(value = "Y")
    public double getY() {
        return m_v.get(1, 0);
    }
@@ -167,12 +167,33 @@ public class Quaternion {
     *
     * @return Z component of the quaternion.
     */
+    @JsonProperty(value = "Z")
    public double getZ() {
        return m_v.get(2, 0);
    }

-    @Override
-    public String toString() {
-        return "Quaternion{" + "m_r=" + m_r + ", m_v=" + Arrays.toString(m_v.getData()) + '}';
+    /**
+     * Returns the rotation vector representation of this quaternion.
+     *
+     * <p>This is also the log operator of SO(3).
+     *
+     * @return The rotation vector representation of this quaternion.
+     */
+    public Vector<N3> toRotationVector() {
+        // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
+        // Sound State Representation through Encapsulation of Manifolds"
+        //
+        // https://arxiv.org/pdf/1107.1119.pdf
+        double norm = m_v.norm();
+
+        if (norm < 1e-9) {
+            return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW()));
+        } else {
+            if (getW() < 0.0) {
+                return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm);
+            } else {
+                return m_v.times(2.0 * Math.atan2(norm, getW()) / norm);
+            }
+        }
    }
 }
--- a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
+++ b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
@@ -17,6 +17,10 @@

 package edu.wpi.first.math.geometry;

+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
@@ -25,7 +29,9 @@ import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;

-/** A rotation in a 3D coordinate. */
+/** A rotation in a 3D coordinate frame represented by a quaternion. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation3d implements Interpolatable<Rotation3d> {
    private Quaternion m_q = new Quaternion();

@@ -37,7 +43,8 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
     *
     * @param q The quaternion.
     */
-    public Rotation3d(Quaternion q) {
+    @JsonCreator
+    public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) {
        m_q = q.normalize();
    }

@@ -174,6 +181,7 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
     *
     * @return The quaternion representation of the Rotation3d.
     */
+    @JsonProperty(value = "quaternion")
    public Quaternion getQuaternion() {
        return m_q;
    }
--- a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
+++ b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
@@ -17,6 +17,10 @@

 package edu.wpi.first.math.geometry;

+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
@@ -28,7 +32,8 @@ import java.util.Objects;
 * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
 * positive Z.
 */
-@SuppressWarnings({"ParameterName", "MemberName"})
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Translation3d implements Interpolatable<Translation3d> {
    private final double m_x;
    private final double m_y;
@@ -46,7 +51,11 @@ public class Translation3d implements Interpolatable<Translation3d> {
     * @param y The y component of the translation.
     * @param z The z component of the translation.
     */
-    public Translation3d(double x, double y, double z) {
+    @JsonCreator
+    public Translation3d(
+            @JsonProperty(required = true, value = "x") double x,
+            @JsonProperty(required = true, value = "y") double y,
+            @JsonProperty(required = true, value = "z") double z) {
        m_x = x;
        m_y = y;
        m_z = z;
@@ -84,6 +93,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
     *
     * @return The X component of the translation.
     */
+    @JsonProperty
    public double getX() {
        return m_x;
    }
@@ -93,6 +103,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
     *
     * @return The Y component of the translation.
     */
+    @JsonProperty
    public double getY() {
        return m_y;
    }
@@ -102,6 +113,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
     *
     * @return The Z component of the translation.
     */
+    @JsonProperty
    public double getZ() {
        return m_z;
    }
--- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
+++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
@@ -32,6 +32,9 @@ public class PhotonPipelineResult {
    // Latency in milliseconds.
    private double latencyMillis;

+    // Timestamp in milliseconds.
+    private double timestampSeconds = -1;
+
    /** Constructs an empty pipeline result. */
    public PhotonPipelineResult() {}

@@ -83,6 +86,25 @@ public class PhotonPipelineResult {
        return latencyMillis;
    }

+    /**
+     * Returns the estimated time the frame was taken, This is more accurate than using <code>
+     * getLatencyMillis()</code>
+     *
+     * @return The timestamp in seconds, or -1 if this result has no timestamp set.
+     */
+    public double getTimestampSeconds() {
+        return timestampSeconds;
+    }
+
+    /**
+     * Sets the FPGA timestamp of this result in seconds.
+     *
+     * @param timestampSeconds The timestamp in seconds.
+     */
+    public void setTimestampSeconds(double timestampSeconds) {
+        this.timestampSeconds = timestampSeconds;
+    }
+
    /**
     * Returns whether the pipeline has targets.
     *
--- a/photon-thinclient/thinclient.html
+++ b/photon-thinclient/thinclient.html
@@ -1,223 +0,0 @@
-<!DOCTYPE html>
-<html>
-
-<head>
-    <title>ThinClient</title>
-
-    <style>
-        * {
-            margin: 0;
-            padding: 0;
-        }
-        .imgbox {
-            display: grid;
-            height: 100%;
-            width: 100%;
-        }
-        .center-fit {
-
-            width: 90vw;
-            margin: auto;
-        }
-    </style>
-
-</head>
-
-<body>
-    <hr>
-        <div class="imgbox">
-            <img id="streamImg" class="center-fit" src=''>
-        </div>
-    <hr>
-
-    <form id="frm1">
-        Host <input type="text" id="host" value="photonvision.local"><br>
-        Port <input type="text" id="port" value="1181"><br>
-    </form>
-
-    <button>Start Stream</button>
-
-    <script type="module">
-        class WebsocketVideoStream{
-
-            constructor(drawDiv, streamPort, host) {
-
-                this.drawDiv = drawDiv;
-                this.image = document.getElementById(this.drawDiv);
-                this.streamPort = streamPort;
-                this.serverAddr = "ws://" + host + "/websocket_cameras";
-                this.noStream = false;
-                this.noStreamPrev = false;
-                this.setNoStream();
-                this.ws_connect();
-                this.imgData = null;
-                this.imgDataTime = -1;
-                this.imgObjURL = null;
-                this.frameRxCount = 0;
-
-                requestAnimationFrame(()=>this.animationLoop());
-            }
-
-            animationLoop(){
-                var now = window.performance.now();
-
-                if((now - this.imgDataTime) > 2500 && this.imgData != null){
-                    //Handle websocket send timeouts by restarting
-                    this.setNoStream();
-                    this.stopStream();
-                    setTimeout(this.startStream.bind(this), 1000); //restart stream one second later
-                } else {
-                    if(this.streamPort == null){
-                        this.setNoStream();
-                    } else if (this.imgData != null) {
-                        //From https://stackoverflow.com/questions/67507616/set-image-src-from-image-blob/67507685#67507685
-                        if(this.imgObjURL != null){
-                            URL.revokeObjectURL(this.imgObjURL)
-                        }
-                        this.imgObjURL = URL.createObjectURL(this.imgData);
-
-                        //Update the image with the new mimetype and image
-                        this.image.src = this.imgObjURL;
-                        this.noStream = false;
-
-                    } else {
-                        //Nothing, hold previous image while waiting for next frame
-                    }
-                }
-
-                requestAnimationFrame(()=>this.animationLoop());
-            }
-
-            setNoStream() {
-                this.noStreamPrev = this.noStream;
-                this.noStream = true;
-                if(this.noStreamPrev == false && this.noStream == true){
-                    //One-shot background change to preserve animation
-                    this.image.src = "loading.gif";
-                }
-            }
-
-            startStream() {
-                if(this.serverConnectionActive == true && this.streamPort > 0){
-                    this.ws.send(JSON.stringify({"cmd": "subscribe", "port":this.streamPort}));
-                    this.noStream = false;
-                }
-            }
-
-            stopStream() {
-                if(this.serverConnectionActive == true && this.streamPort > 0){
-                    this.ws.send(JSON.stringify({"cmd": "unsubscribe"}));
-                    this.noStream = true;
-                }
-            }
-
-            setPort(streamPort){
-                this.stopStream();
-                this.frameRxCount = 0;
-                this.streamPort = streamPort;
-                this.startStream();
-            }
-
-            ws_onOpen() {
-                // Set the flag allowing general server communication
-                this.serverConnectionActive = true;
-                console.log("Connected!");
-                this.startStream();
-            }
-
-            ws_onClose(e) {
-                this.setNoStream();
-
-                //Clear flags to stop server communication
-                this.ws = null;
-                this.serverConnectionActive = false;
-
-                console.log('Camera 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){
-                e; //prevent unused failure
-                this.ws.close();
-            }
-
-            ws_onMessage(e){
-                if(typeof e.data === 'string'){
-                    //string data from host
-                    //TODO - anything to recieve info here? Maybe "avaialble streams?"
-                } else {
-                    if(e.data.size > 0){
-                        //binary data - a frame
-                        this.imgData = e.data;
-                        this.imgDataTime = window.performance.now();
-                        this.frameRxCount++;
-                    } else {
-                        //TODO - server is sending empty frames?
-                    }
-
-                }
-
-            }
-
-            ws_connect() {
-                this.ws = new WebSocket(this.serverAddr);
-                this.ws.binaryType = "blob";
-                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("Connecting to server " + this.serverAddr);
-            }
-
-            ws_close(){
-                this.ws.close();
-            }
-
-        }
-
-        var stream = null;
-
-        function streamStartRequest() {
-            var host = document.getElementById("host").value + ":5800";
-            var port = document.getElementById("port").value;
-            if(stream == null){
-                stream = new WebsocketVideoStream("streamImg",port,host);
-                stream.startStream();
-            } else {
-                stream.setPort(port);
-            }
-
-        }
-
-        // Attach listener
-        document.querySelector('button').addEventListener('click', streamStartRequest);
-
-        // Deal with URLParams, validating inputs
-        const queryString = window.location.search;
-        const urlParams = new URLSearchParams(queryString);
-        const port_in = urlParams.get('port')
-        const host_in = urlParams.get('host')
-        if(port_in != ""){
-            document.getElementById("port").value = port_in;
-        }
-
-        if(host_in != ""){
-            document.getElementById("host").value = host_in;
-        }
-
-        if(port_in != "" & host_in != ""){
-            streamStartRequest(); //we got valid inputs, auto-start the stream
-        }
-
-
-    </script>
-
-</body>
-
-
-</html>
--- a/photonlib-cpp-examples/src/main/cpp/examples/examples.json
+++ b/photonlib-cpp-examples/src/main/cpp/examples/examples.json
@@ -35,16 +35,4 @@
    "dependencies": [],
    "foldername": "aimandrange"
  }
-  {
-    "name": "SimAimAndRange",
-    "description": "Adding Simulation Support to the Aim And Range example",
-    "tags": [],
-    "gradlebase": "cpp",
-    "language": "cpp",
-    "commandversion": 1,
-    "mainclass": "Main",
-    "packagetoreplace": null,
-    "dependencies": [],
-    "foldername": "simaimandrange"
-  }
 ]
--- a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/Robot.cpp
+++ b/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/Robot.cpp
@@ -1,73 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2022 PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include "Robot.h"
-
-#include <photonlib/PhotonUtils.h>
-
-void Robot::TeleopPeriodic() {
-  double forwardSpeed;
-  double rotationSpeed;
-
-  if (xboxController.GetAButton()) {
-    // Vision-alignment mode
-    // Query the latest result from PhotonVision
-    const auto& result = camera.GetLatestResult();
-
-    if (result.HasTargets()) {
-      // First calculate range
-      units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget(
-          CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
-          units::degree_t{result.GetBestTarget().GetPitch()});
-
-      // Use this range as the measurement we give to the PID controller.
-      // -1.0 required to ensure positive PID controller effort _increases_
-      // range
-      forwardSpeed = -forwardController.Calculate(range.value(),
-                                                  GOAL_RANGE_METERS.value());
-
-      // Also calculate angular power
-      // -1.0 required to ensure positive PID controller effort _increases_ yaw
-      rotationSpeed =
-          -turnController.Calculate(result.GetBestTarget().GetYaw(), 0);
-    } else {
-      // If we have no targets, stay still.
-      forwardSpeed = 0;
-      rotationSpeed = 0;
-    }
-  } else {
-    // Manual Driver Mode
-    forwardSpeed = -xboxController.GetRightY();
-    rotationSpeed = xboxController.GetLeftX();
-  }
-
-  // Use our forward/turn speeds to control the drivetrain
-  drive.ArcadeDrive(forwardSpeed, rotationSpeed);
-}
-
-void Robot::SimulationPeriodic() { dtSim.update(); }
-
-#ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
-#endif
--- a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/sim/DrivetrainSim.cpp
+++ b/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/sim/DrivetrainSim.cpp
@@ -1,50 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2022 PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include "DrivetrainSim.h"
-
-/**
- * Perform all periodic drivetrain simulation related tasks to advance our
- * simulation of robot physics forward by a single 20ms step.
- */
-void DrivetrainSim::update() {
-  double leftMotorCmd = 0;
-  double rightMotorCmd = 0;
-
-  if (frc::DriverStation::IsEnabled() &&
-      !frc::RobotController::IsBrownedOut()) {
-    leftMotorCmd = leftLeader.GetSpeed();
-    rightMotorCmd = rightLeader.GetSpeed();
-  }
-
-  m_drivetrainSimulator.SetInputs(
-      units::volt_t(leftMotorCmd * frc::RobotController::GetInputVoltage()),
-      units::volt_t(-rightMotorCmd * frc::RobotController::GetInputVoltage()));
-  m_drivetrainSimulator.Update(20_ms);
-
-  // Update PhotonVision based on our new robot position.
-  simVision.ProcessFrame(m_drivetrainSimulator.GetPose());
-
-  field.SetRobotPose(m_drivetrainSimulator.GetPose());
-}
--- a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/DrivetrainSim.h
+++ b/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/DrivetrainSim.h
@@ -1,111 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2022 PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include <photonlib/SimVisionSystem.h>
-
-#include <frc/DriverStation.h>
-#include <frc/RobotController.h>
-#include <frc/geometry/Pose2d.h>
-#include <frc/geometry/Rotation2d.h>
-#include <frc/geometry/Translation2d.h>
-#include <frc/simulation/DifferentialDrivetrainSim.h>
-#include <frc/simulation/PWMSim.h>
-#include <frc/smartdashboard/Field2d.h>
-#include <frc/smartdashboard/SmartDashboard.h>
-#include <frc/system/plant/LinearSystemId.h>
-#include <units/angle.h>
-#include <units/length.h>
-#include <units/time.h>
-
-#pragma once
-
-class DrivetrainSim {
- public:
-  DrivetrainSim() {
-    simVision.AddSimVisionTarget(photonlib::SimVisionTarget(
-        farTargetPose, 81.91_in, targetWidth, targetHeight));
-    frc::SmartDashboard::PutData("Field", &field);
-  }
-
-  void update();
-
- private:
-  // Simulated Motor Controllers
-  frc::sim::PWMSim leftLeader{0};
-  frc::sim::PWMSim rightLeader{1};
-
-  // Simulation Physics
-  // Configure these to match your drivetrain's physical dimensions
-  // and characterization results.
-  static constexpr decltype(1_V / 1_mps) kv = 1.98 * 1_V * 1_s / 1_m;
-  static constexpr decltype(1_V / 1_mps_sq) ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
-  static constexpr decltype(1_V / 1_rad_per_s) kvAngular =
-      1.5 * 1_V * 1_s / 1_rad;
-  static constexpr decltype(1_V / 1_rad_per_s_sq) kaAngular =
-      0.3 * 1_V * 1_s * 1_s / 1_rad;
-  static constexpr units::meter_t kTrackWidth = 1_m;
-
-  const frc::LinearSystem<2, 2, 2> kDrivetrainPlant =
-      frc::LinearSystemId::IdentifyDrivetrainSystem(kv, ka, kvAngular,
-                                                    kaAngular, kTrackWidth);
-
-  frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
-      kDrivetrainPlant,     2.0_ft,
-      frc::DCMotor::CIM(2), 8.0,
-      6.0_in / 2,           {0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
-
-  // Simulated Vision System.
-  // Configure these to match your PhotonVision Camera,
-  // pipeline, and LED setup.
-  units::degree_t camDiagFOV = 170.0_deg;  // assume wide-angle camera
-  units::meter_t camHeightOffGround = 24_in;
-  units::meter_t maxLEDRange = 20_m;
-  int camResolutionWidth = 640;   // pixels
-  int camResolutionHeight = 480;  // pixels
-  double minTargetArea = 10;      // square pixels
-
-  photonlib::SimVisionSystem simVision{"photonvision",      camDiagFOV,
-                                       frc::Transform2d{},  camHeightOffGround,
-                                       maxLEDRange,         camResolutionWidth,
-                                       camResolutionHeight, minTargetArea};
-
-  // See
-  // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
-  // page 208
-  const units::meter_t targetWidth = 41.30_in - 6.70_in;
-  // See
-  // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
-  // page 197
-  const units::meter_t targetHeight = 98.19_in - 81.19_in;  // meters
-  // See
-  // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
-  // pages 4 and 5
-  const units::meter_t tgtXPos = 54_ft;
-  const units::meter_t tgtYPos = (27.0_ft / 2) - 43.75_in - (48.0_in / 2.0);
-  const frc::Translation2d targetTrans{tgtXPos, tgtYPos};
-  const frc::Rotation2d targetRot{0.0_deg};
-  frc::Pose2d farTargetPose{targetTrans, targetRot};
-
-  frc::Field2d field{};
-};
--- a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/Robot.h
+++ b/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/Robot.h
@@ -1,77 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2022 PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#pragma once
-
-#include <photonlib/PhotonCamera.h>
-
-#include <frc/TimedRobot.h>
-#include <frc/XboxController.h>
-#include <frc/controller/PIDController.h>
-#include <frc/drive/DifferentialDrive.h>
-#include <frc/motorcontrol/PWMVictorSPX.h>
-#include <units/angle.h>
-#include <units/length.h>
-
-#include "DrivetrainSim.h"
-
-class Robot : public frc::TimedRobot {
- public:
-  void TeleopPeriodic() override;
-  void SimulationPeriodic() override;
-
- private:
-  // Constants such as camera and target height stored. Change per robot and
-  // goal!
-  const units::meter_t CAMERA_HEIGHT = 24_in;
-  const units::meter_t TARGET_HEIGHT = 81.19_in;
-
-  // Angle between horizontal and the camera.
-  const units::radian_t CAMERA_PITCH = 15_deg;
-
-  // How far from the target we want to be
-  const units::meter_t GOAL_RANGE_METERS = 10_ft;
-
-  // PID constants should be tuned per robot
-  const double LINEAR_P = 2.0;
-  const double LINEAR_D = 0.0;
-  frc2::PIDController forwardController{LINEAR_P, 0.0, LINEAR_D};
-
-  const double ANGULAR_P = 0.03;
-  const double ANGULAR_D = 0.003;
-  frc2::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D};
-
-  // Change this to match the name of your camera
-  photonlib::PhotonCamera camera{"photonvision"};
-
-  frc::XboxController xboxController{0};
-
-  // Drive motors
-  frc::PWMVictorSPX leftMotor{0};
-  frc::PWMVictorSPX rightMotor{1};
-  frc::DifferentialDrive drive{leftMotor, rightMotor};
-
-  // Simulation Support
-  DrivetrainSim dtSim{};
-};
--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/sim/DrivetrainSim.java
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/sim/DrivetrainSim.java
@@ -25,9 +25,10 @@
 package org.photonlib.examples.simaimandrange.sim;

 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -73,7 +74,7 @@ public class DrivetrainSim {
    // Configure these to match your PhotonVision Camera,
    // pipeline, and LED setup.
    double camDiagFOV = 170.0; // degrees - assume wide-angle camera
-    double camPitch = Units.radiansToDegrees(Robot.CAMERA_PITCH_RADIANS); // degrees
+    double camPitch = Robot.CAMERA_PITCH_RADIANS; // degrees
    double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters
    double maxLEDRange = 20; // meters
    int camResolutionWidth = 640; // pixels
@@ -84,9 +85,8 @@ public class DrivetrainSim {
            new SimVisionSystem(
                    "photonvision",
                    camDiagFOV,
-                    camPitch,
-                    new Transform2d(),
-                    camHeightOffGround,
+                    new Transform3d(
+                            new Translation3d(0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)),
                    maxLEDRange,
                    camResolutionWidth,
                    camResolutionHeight,
@@ -105,13 +105,15 @@ public class DrivetrainSim {
    double tgtXPos = Units.feetToMeters(54);
    double tgtYPos =
            Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0);
-    Pose2d farTargetPose = new Pose2d(new Translation2d(tgtXPos, tgtYPos), new Rotation2d(0.0));
+    Pose3d farTargetPose =
+            new Pose3d(
+                    new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS),
+                    new Rotation3d(0.0, 0.0, 0.0));

    Field2d field = new Field2d();

    public DrivetrainSim() {
-        simVision.addSimVisionTarget(
-                new SimVisionTarget(farTargetPose, Robot.TARGET_HEIGHT_METERS, targetWidth, targetHeight));
+        simVision.addSimVisionTarget(new SimVisionTarget(farTargetPose, targetWidth, targetHeight, -1));
        SmartDashboard.putData("Field", field);
    }

--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java
@@ -87,7 +87,6 @@ public class Constants {
    // page 197
    public static final double targetHeight =
            Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters
-    public static final double targetHeightAboveGround = Units.inchesToMeters(81.19); // meters

    // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
    // pages 4 and 5
@@ -103,6 +102,5 @@ public class Constants {
                    new Rotation3d(0.0, 0.0, Units.degreesToRadians(180)));

    public static final SimVisionTarget kFarTarget =
-            new SimVisionTarget(
-                    kFarTargetPose.toPose2d(), targetHeightAboveGround, targetWidth, targetHeight);
+            new SimVisionTarget(kFarTargetPose, targetWidth, targetHeight, 42);
 }
--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java
@@ -34,7 +34,6 @@ import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.numbers.N5;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.AnalogGyro;
-import edu.wpi.first.wpilibj.Timer;
 import org.photonvision.PhotonCamera;

 /**
@@ -84,7 +83,7 @@ public class DrivetrainPoseEstimator {

        var res = cam.getLatestResult();
        if (res.hasTargets()) {
-            var imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000.0;
+            var imageCaptureTime = res.getTimestampSeconds();
            var camToTargetTrans = res.getBestTarget().getBestCameraToTarget();
            var camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse());
            m_poseEstimator.addVisionMeasurement(
--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java
@@ -89,11 +89,7 @@ public class DrivetrainSim {
            new SimVisionSystem(
                    Constants.kCamName,
                    camDiagFOV,
-                    camPitch,
-                    new Transform2d(
-                            Constants.kCameraToRobot.getTranslation().toTranslation2d(),
-                            Constants.kCameraToRobot.getRotation().toRotation2d()),
-                    camHeightOffGround,
+                    Constants.kCameraToRobot,
                    maxLEDRange,
                    camResolutionWidth,
                    camResolutionHeight,
Author	SHA1	Message	Date
Matt	dcad7f34a2	Fix thinclient address in dev builds and move thinclient (#586 ) * Fix thinclient address in dev builds and move thinclient Update USBFrameProvider.java Create index.html Update index.html Null check stream to prevent spam * Update main.yml Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>	2022-11-13 15:51:02 -06:00
Matt	72d8f49145	Add orthogonalizeRotationMatrix (#587 ) * Add orthogonalizeRotationMatrix * Update docstring Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>	2022-11-13 14:49:59 -06:00
Matt	df852410b0	Disable 3d if new resolution is uncalibrated (#591 ) Closes #590 Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>	2022-11-13 14:32:49 -06:00
Matt	3c7165bb0d	Force photonlib JSON to regenerate every build (#589 )	2022-11-13 14:07:15 -06:00
Ethan Frank	f193a2331a	Fix sim versionEntry NT table path (#569 ) * Fix sim versionEntry NT table path * Fix compile issue that mainTable is not accessible form SimPhotonCamera * Fix format Co-authored-by: Matt <matthew.morley.ca@gmail.com> Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>	2022-11-13 13:18:55 -06:00
Matt	c7aa84ca41	Add tag16h5 support (#584 ) * Fix target dropdown * Add hamming and decision margin to UI, pipeline * Run spotless * Update index.html * Update index.html * Implement second apriltag size	2022-11-10 18:48:41 -08:00
Matt	209cdbf45f	Re-license apriltag code (#585 ) Relicense under wpilib BSD license	2022-11-09 23:22:47 -05:00
Chris Gerth	e03ec862a8	disallowed non-integer decimation (#573 )	2022-11-09 22:01:58 -06:00
Chris Gerth	8169da5ad4	wip getting stream divisor to update properly (#574 )	2022-11-09 23:01:11 -05:00
shueja-personal	916431b4ff	JSONify the bundled 3D geometry (#578 ) Co-authored-by: Matt M <matthew.morley.ca@gmail.com>	2022-11-09 14:00:46 -05:00
Noah	7dd1719fbd	Expose NT entry change time in PhotonLib (#562 ) Adds target change timestamp to PhotonPipelineResult Co-authored-by: Matt <matthew.morley.ca@gmail.com>	2022-11-07 13:09:55 -05:00
Chris Gerth	b408a58e9e	Sim Updates for 2023 (#512 ) * WIP updating sim stuff for 2023 and pose3d's * vision system build fixups, but test not yet passing. * WIP Sim fixups and working on testcases * Still doesn't work, but closer * tests pass * removed C++ sim support * formatting update * adjusted target height above ground per review * Turns out its unused * missed example removal	2022-11-03 15:05:37 -05:00
Chris Gerth	a64697e714	Added proper state machine to websocket video stream to control connect/disconnect sequence better. (#561 )	2022-11-03 15:05:17 -05:00
Jack	e971db2f52	Fix pipeline setting update not being sent if only autoexposure fails (#565 )	2022-11-03 13:12:21 -04:00
Matt	7b6afd545b	Pull thinclient into built JAR (#558 )	2022-10-31 16:18:02 -04:00