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>

.github/workflows/main.yml

@@ -24,25 +24,20 @@ jobs:
     # The type of runner that the job will run on.
     runs-on: ubuntu-latest
 
-    # Grab the docker container.
-    container:
-      image: docker://node:10
-
     steps:
     # Checkout code.
-    - uses: actions/checkout@v1
+    - uses: actions/checkout@v3
 
     # Setup Node.js
     - name: Setup Node.js
-      uses: actions/setup-node@v3.4.1
+      uses: actions/setup-node@v3
       with:
-        node-version: 14
+        node-version: 16
 
     # Run npm
-    - run: |
-        npm install -g npm
-        npm ci
-        npm run build --if-present
+    - run: npm update -g npm
+    - run: npm ci
+    - run: npm run build --if-present
 
     # Upload client artifact.
     - uses: actions/upload-artifact@master
@@ -57,7 +52,9 @@ jobs:
     steps:
       # Checkout code.
       - name: Checkout code
-        uses: actions/checkout@v1
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
 
       # Fetch tags.
       - name: Fetch tags
@@ -65,9 +62,10 @@ jobs:
 
       # Install Java 11.
       - name: Install Java 11
-        uses: actions/setup-java@v1
+        uses: actions/setup-java@v3
         with:
           java-version: 11
+          distribution: temurin
 
       # Run Gradle build.
       - name: Gradle Build
@@ -85,12 +83,12 @@ jobs:
 
       # Publish Coverage Report.
       - name: Publish Server Coverage Report
-        uses: codecov/codecov-action@v1
+        uses: codecov/codecov-action@v3
         with:
           file: ./photon-server/build/reports/jacoco/test/jacocoTestReport.xml
 
       - name: Publish Core Coverage Report
-        uses: codecov/codecov-action@v1
+        uses: codecov/codecov-action@v3
         with:
           file: ./photon-core/build/reports/jacoco/test/jacocoTestReport.xml
 
@@ -99,13 +97,13 @@ jobs:
 
     steps:
       # Checkout docs.
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           repository: 'PhotonVision/photonvision-docs.git'
           ref: master
 
       # Install Python.
-      - uses: actions/setup-python@v2
+      - uses: actions/setup-python@v4
         with:
           python-version: '3.6'
 
@@ -136,12 +134,15 @@ jobs:
 
     steps:
       # Checkout code.
-      - uses: actions/checkout@v1
+      - uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
 
       # Install Java 11.
-      - uses: actions/setup-java@v1
+      - uses: actions/setup-java@v3
         with:
           java-version: 11
+          distribution: temurin
 
       # Check server code with Spotless.
       - run: |
@@ -168,12 +169,13 @@ jobs:
     runs-on: ${{ matrix.os }}
     name: "Photonlib - Build - ${{ matrix.artifact-name }}"
     steps:
-      - uses: actions/checkout@v2.3.4
+      - uses: actions/checkout@v3
         with:
           fetch-depth: 0
-      - uses: actions/setup-java@v1
+      - uses: actions/setup-java@v3
         with:
           java-version: 11
+          distribution: temurin
       - run: git fetch --tags --force
       - run: |
           chmod +x gradlew
@@ -200,12 +202,13 @@ jobs:
     container: ${{ matrix.container }}
     name: "Photonlib - Build - ${{ matrix.artifact-name }}"
     steps:
-      - uses: actions/checkout@v2.3.4
+      - uses: actions/checkout@v3
         with:
           fetch-depth: 0
-      - uses: actions/setup-java@v1
+      - uses: actions/setup-java@v3
         with:
           java-version: 11
+          distribution: temurin
       - run: |
           chmod +x gradlew
           ./gradlew photon-lib:build --max-workers 1
@@ -220,14 +223,14 @@ jobs:
     name: "wpiformat"
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
       - name: Fetch all history and metadata
         run: |
           git fetch --prune --unshallow
           git checkout -b pr
           git branch -f master origin/master
       - name: Set up Python 3.8
-        uses: actions/setup-python@v2
+        uses: actions/setup-python@v4
         with:
           python-version: 3.8
       - name: Install clang-format
@@ -244,7 +247,7 @@ jobs:
       - name: Generate diff
         run: git diff HEAD > wpiformat-fixes.patch
         if: ${{ failure() }}
-      - uses: actions/upload-artifact@v2
+      - uses: actions/upload-artifact@v3
         with:
           name: wpiformat fixes
           path: wpiformat-fixes.patch
@@ -258,12 +261,15 @@ jobs:
 
     steps:
       # Checkout code.
-      - uses: actions/checkout@v1
+      - uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
 
       # Install Java 11.
-      - uses: actions/setup-java@v1
+      - uses: actions/setup-java@v3
         with:
           java-version: 11
+          distribution: temurin
 
       # Clear any existing web resources.
       - run: |
@@ -271,13 +277,13 @@ jobs:
           mkdir -p photon-server/src/main/resources/web/docs
 
       # Download client artifact to resources folder.
-      - uses: actions/download-artifact@v2
+      - uses: actions/download-artifact@v3
         with:
           name: built-client
           path: photon-server/src/main/resources/web/
 
       # Download docs artifact to resources folder.
-      - uses: actions/download-artifact@v2
+      - uses: actions/download-artifact@v3
         with:
           name: built-docs
           path: photon-server/src/main/resources/web/docs
@@ -296,14 +302,15 @@ jobs:
           ./scripts/generatePiImage.sh
 
       # Upload final fat jar as artifact.
-      - uses: actions/upload-artifact@master
+      - uses: actions/upload-artifact@v3
         with:
-          name: jar
+          name: jars
           path: photon-server/build/libs
-      - uses: actions/upload-artifact@master
+      - uses: actions/upload-artifact@v3
+        if: github.event_name != 'pull_request'
         with:
           name: image
-          path: photonvision*.zip
+          path: photonvision*.xz
 
       - uses: pyTooling/Actions/releaser@r0
         with:
@@ -312,7 +319,7 @@ jobs:
           rm: true
           files: |
             photon-server/build/libs/*.jar
-            photonvision*.zip
+            photonvision*.xz
         if: github.event_name == 'push'
 
   photon-release:
@@ -323,7 +330,7 @@ jobs:
       # This *should* pull in fat and pi-only jars
       - uses: actions/download-artifact@v2
         with:
-          name: jar
+          name: jars
 
       # And the image we made previously
       - uses: actions/download-artifact@v2

.gitignore

@@ -30,6 +30,7 @@ backend/settings/
 *.nar
 *.ear
 *.zip
+*.xz
 *.tar.gz
 *.rar
 

.styleguide

@@ -13,6 +13,7 @@ modifiableFileExclude {
   \.jpg$
   \.jpeg$
   \.png$
+  \.gif$
   \.so$
   \.dll$
 }

build.gradle

@@ -11,7 +11,7 @@ plugins {
 
 allprojects {
     repositories {
-        jcenter()
+        mavenCentral()
         maven { url = "https://maven.photonvision.org/repository/internal/" }
     }
     wpilibRepositories.addAllReleaseRepositories(it)

photon-client/package-lock.json

@@ -19,8 +19,7 @@
         "three-full": "^28.0.2",
         "vue": "^2.6.12",
         "vue-axios": "^2.1.5",
-        "vue-native-websocket": "git+https://github.com/PhotonVision/vue-native-websocket.git#5189f29",
-        "vue-router": "^3.4.3",
+        "vue-native-websocket": "git+https://github.com/PhotonVision/vue-native-websocket.git#5189f29",        "vue-router": "^3.4.3",
         "vuetify": "^2.3.10",
         "vuex": "^3.5.1"
       },
@@ -25271,8 +25270,7 @@
     },
     "vue-native-websocket": {
       "version": "git+ssh://git@github.com/PhotonVision/vue-native-websocket.git#7a327918e03b215b6899b0d648c5130ece1fa912",
-      "from": "vue-native-websocket@git+https://github.com/PhotonVision/vue-native-websocket.git#7a32791"
-    },
+      "from": "vue-native-websocket@git+https://github.com/PhotonVision/vue-native-websocket.git#7a32791"    },
     "vue-router": {
       "version": "3.4.3"
     },

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>

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

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

@@ -5,7 +5,7 @@
     :style="styleObject"
     :src="src"
     alt=""
-    @click="e => $emit('click', e)"
+    @click="e => {this.openThinclientStream(e)}"
   >
 </template>
 
@@ -13,10 +13,11 @@
     export default {
         name: "CvImage",
         // eslint-disable-next-line vue/require-prop-types
-        props: ['address', 'scale', 'maxHeight', 'maxHeightMd', 'maxHeightLg', 'maxHeightXl', 'colorPicking', 'id', 'disconnected'],
+        props: ['idx', 'scale', 'maxHeight', 'maxHeightMd', 'maxHeightLg', 'maxHeightXl', 'colorPicking', 'id', 'disconnected'],
         data() {
             return {
                 seed: 1.0,
+                src: ""
             }
         },
         computed: {
@@ -46,18 +47,47 @@
                     return ret;
                 }
             },
-            src: {
+            port: {
               get() {
-                return this.disconnected ? require("../../assets/noStream.jpg") : this.address + "?" + this.seed // This prevents caching
-              },
-            },
+                if(this.idx == 0){
+                  return this.$store.state.cameraSettings[this.$store.state.currentCameraIndex].inputStreamPort;
+                } else {
+                  return this.$store.state.cameraSettings[this.$store.state.currentCameraIndex].outputStreamPort;
+                }
+              }
+            }
+        },
+        watch : {
+          port(newPort, oldPort){
+            newPort;
+            oldPort;
+            this.reload();
+          },
+          disconnected(newVal, oldVal){
+            oldVal;
+            if(newVal){
+              this.wsStream.setPort(0);
+            } else {
+              this.wsStream.setPort(this.port);
+            }
+          }
         },
         mounted() {
-            this.reload(); // Force reload image on creation
+          var wsvs = require('../../plugins/WebsocketVideoStream');
+          this.wsStream = new wsvs.WebsocketVideoStream(this.id, this.port, this.$address);
+        },
+        unmounted() {
+          this.wsStream.setPort(0);
         },
         methods: {
             reload() {
-                this.seed = new Date().getTime();
+              console.log("Reloading " + this.id + " with port " + String(this.port));
+              this.wsStream.setPort(this.port);
+            },
+            openThinclientStream(e){
+              e;
+              var URL = "/thinclient.html?port=" + String(this.port) + "&host=" + window.location.hostname;
+              window.open(URL, '_blank');
             }
         },
     }

photon-client/src/components/pipeline/3D/MiniMap.vue

@@ -1,11 +1,14 @@
 <template>
-  <div id="MapContainer" style="flex-grow:1">
+  <div
+    id="MapContainer"
+    style="flex-grow:1"
+  >
     <v-row>
       <v-col
         align="center"
         cols="12"
       >
-        <span class="white--text" >Target Location</span>
+        <span class="white--text">Target Location</span>
       </v-col>
     </v-row>
     <v-row>
@@ -14,8 +17,31 @@
         cols="12"
         align-self="stretch"
       >
-        <canvas id="canvasId" style="width:100%;height:100%"/>
+        <canvas
+          id="canvasId"
+          style="width:100%;height:100%"
+        />
       </v-col>
+      <v-row>
+        <v-col>
+          <v-btn
+            class="ml-10"
+            color="secondary"
+            @click="resetCamFirstPerson"
+          >
+            First Person
+          </v-btn>
+        </v-col>
+        <v-col>
+          <v-btn
+            class="ml-10"
+            color="secondary"
+            @click="resetCamThirdPerson"
+          >
+            Third Person
+          </v-btn>
+        </v-col>
+      </v-row>
     </v-row>
   </div>
 </template>
@@ -25,6 +51,7 @@
 import {
   ArrowHelper,
   BoxGeometry,
+  ConeGeometry,
   Mesh,
   MeshNormalMaterial,
   PerspectiveCamera,
@@ -59,13 +86,102 @@ export default {
       }
     },
   },
+
+  mounted() {
+    const scene = new Scene();
+    this.scene = scene;
+    const camera = new PerspectiveCamera(75, 800 / 800, 0.1, 1000);
+    this.camera = camera;
+
+    const canvas = document.getElementById("canvasId"); // getting the canvas element
+    this.canvas = canvas;
+    const renderer = new WebGLRenderer({"canvas": canvas});
+    this.renderer = renderer;
+    scene.background = new Color(0xa9a9a9)
+
+    //Set up resize handlers
+    this.onWindowResize();
+    window.addEventListener( 'resize', this.onWindowResize, false );
+
+    //Add the reference frame cues
+    this.refFrameCues = []
+    // coordinate system
+    this.refFrameCues.push(new ArrowHelper(new Vector3(1, 0, 0).normalize(), new Vector3(0, 0, 0),
+        1, // length
+        0xff0000,
+        0.1,
+        0.1,
+    ))
+    this.refFrameCues.push(new ArrowHelper(new Vector3(0, 1, 0).normalize(), new Vector3(0, 0, 0),
+        1, // length
+        0x00ff00,
+        0.1,
+        0.1,
+    ))
+    this.refFrameCues.push(new ArrowHelper(new Vector3(0, 0, 1).normalize(), new Vector3(0, 0, 0),
+        1, // length
+        0x0000ff,
+        0.1,
+        0.1,
+    ))
+
+    //something that looks vaguely like a camera
+    const camSize = 0.2;
+    const camBodyGeometry = new BoxGeometry(camSize, camSize, camSize);
+    const camLensGeometry = new ConeGeometry(camSize*0.4, camSize*0.8, 30);
+    const camMaterial = new MeshNormalMaterial();
+    const camBody = new Mesh(camBodyGeometry, camMaterial);
+    const camLens = new Mesh(camLensGeometry, camMaterial);
+    camBody.position.set(0,0,0);
+    camLens.rotateZ(Math.PI / 2);
+    camLens.position.set(camSize*0.8,0,0);
+    this.refFrameCues.push(camBody)
+    this.refFrameCues.push(camLens)
+
+    var controls = new TrackballControls(
+        camera,
+        renderer.domElement
+    );
+    controls.rotateSpeed = 1.0;
+    controls.zoomSpeed = 1.2;
+    controls.panSpeed = 0.8;
+    controls.noZoom = false;
+    controls.noPan = false;
+    controls.staticMoving = true;
+    controls.dynamicDampingFactor = 0.3;
+    controls.keys = [65, 83, 68];
+    this.controls = controls;
+
+    this.scene.add(...this.refFrameCues)
+    this.resetCamFirstPerson();
+
+    controls.update();
+
+    function animate() {
+      requestAnimationFrame(animate);
+
+      controls.update();
+      renderer.render(scene, camera);
+
+      //camera.updateMatrixWorld();
+      //console.log("================")
+      //console.log(camera.position);
+      //console.log(camera.rotation);
+      //console.log(camera.up);
+
+    }
+
+    this.drawTargets()
+
+    animate();
+  },
   methods: {
     drawTargets() {
       this.scene.remove(...this.cubes)
       this.cubes = []
 
       for (const target of this.targets) {
-        const geometry = new BoxGeometry(0.2, 0.2, 0.3 / 5);
+        const geometry = new BoxGeometry(0.3 / 5, 0.2, 0.2);
         const material = new MeshNormalMaterial();
         let quat = (new Quaternion(
             target.pose.qx,
@@ -113,6 +229,7 @@ export default {
       if(this.cubes.length > 0)
         this.scene.add(...this.cubes);
     },
+
     onWindowResize() {
       var container = document.getElementById("MapContainer")
       if(container){
@@ -123,72 +240,24 @@ export default {
         this.renderer.setSize( this.canvas.width, this.canvas.height );
       }
     },
-  },
-
-  mounted() {
-    const scene = new Scene();
-    this.scene = scene;
-    const camera = new PerspectiveCamera(75, 800 / 800, 0.1, 1000);
-    this.camera = camera;
-
-    const canvas = document.getElementById("canvasId"); // getting the canvas element
-    this.canvas = canvas;
-    const renderer = new WebGLRenderer({"canvas": canvas});
-    this.renderer = renderer;
-    scene.background = new Color(0xa9a9a9)
-
-
-    this.onWindowResize();
-    window.addEventListener( 'resize', this.onWindowResize, false );
-
-    scene.add(new ArrowHelper(new Vector3(1, 0, 0).normalize(), new Vector3(0, 0, 0),
-        1, // length
-        0xff0000,
-        0.5,
-        0.5,
-    ))
-    scene.add(new ArrowHelper(new Vector3(0, 1, 0).normalize(), new Vector3(0, 0, 0),
-        1, // length
-        0x00ff00,
-        0.5,
-        0.5,
-    ))
-    scene.add(new ArrowHelper(new Vector3(0, 0, 1).normalize(), new Vector3(0, 0, 0),
-        1, // length
-        0x0000ff,
-        0.5,
-        0.5,
-    ))
-
-    var controls = new TrackballControls(
-        camera,
-        renderer.domElement
-    );
-    controls.rotateSpeed = 1.0;
-    controls.zoomSpeed = 1.2;
-    controls.panSpeed = 0.8;
-    controls.noZoom = false;
-    controls.noPan = false;
-    controls.staticMoving = true;
-    controls.dynamicDampingFactor = 0.3;
-    controls.keys = [65, 83, 68];
-
-
-    camera.position.set(-0.1,0,0);
-    camera.rotation.set(-90, 0, 90);
-    camera.up.set(0,0,1);
-    controls.update();
-
-    function animate() {
-      requestAnimationFrame(animate);
-
-      controls.update();
-      renderer.render(scene, camera);
-    }
-
-    this.drawTargets()
-
-    animate();
+    resetCamThirdPerson(){
+      //Sets camera to third person position
+      this.controls.reset();
+      this.camera.position.set(-1.39,-1.09,1.17);
+      this.camera.up.set(0,0,1);
+      this.controls.target.set(4.0,0.0,0.0);
+      this.controls.update();
+      this.scene.add(...this.refFrameCues)
+    },
+    resetCamFirstPerson(){
+      //Sets camera to first person position
+      this.controls.reset();
+      this.camera.position.set(-0.1,0,0);
+      this.camera.up.set(0,0,1);
+      this.controls.target.set(0.0,0.0,0.0);
+      this.controls.update();
+      this.scene.remove(...this.refFrameCues)
+    },
   }
 
 

photon-client/src/main.js

@@ -15,11 +15,11 @@ if (process.env.NODE_ENV === "production") {
     Vue.prototype.$address = location.hostname + ":5800";
 }
 
-const wsURL = '//' + Vue.prototype.$address + '/websocket';
+const wsDataURL = '//' + Vue.prototype.$address + '/websocket_data';
 
 import VueNativeSock from 'vue-native-websocket';
 
-Vue.use(VueNativeSock, wsURL, {
+Vue.use(VueNativeSock, wsDataURL, {
     reconnection: true,
     reconnectionDelay: 100,
     connectManually: true,

photon-client/src/plugins/ColorPicker.js

@@ -5,7 +5,7 @@ function initColorPicker() {
     if (!canvas)
         canvas = document.createElement('canvas');
 
-    image = document.querySelector('#normal-stream');
+    image = document.querySelector('#raw-stream');
     if (image !== null) {
       canvas.width = image.width;
       canvas.height = image.height;

photon-client/src/plugins/WebsocketVideoStream.js

@@ -0,0 +1,244 @@
+
+
+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.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;
+
+        // 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("Camera Websockets 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){
+        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?"
+        } 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();
+    }
+
+}
+
+
+export default {WebsocketVideoStream}

photon-client/src/store/index.js

@@ -35,8 +35,8 @@ export default new Vuex.Store({
                 tiltDegrees: 0.0,
                 currentPipelineIndex: 0,
                 pipelineNicknames: ["Unknown"],
-                outputStreamPort: 1181,
-                inputStreamPort: 1182,
+                outputStreamPort: 0,
+                inputStreamPort: 0,
                 nickname: "Unknown",
                 videoFormatList: [
                     {
@@ -57,6 +57,7 @@ export default new Vuex.Store({
                     // Settings that apply to all pipeline types
                     cameraExposure: 1,
                     cameraBrightness: 2,
+                    cameraAutoExposure: false,
                     cameraRedGain: 3,
                     cameraBlueGain: 4,
                     inputImageRotationMode: 0,
@@ -94,7 +95,10 @@ export default new Vuex.Store({
                     blur: 0.0,
                     threads: 1,
                     debug: false,
-                    refineEdges: true
+                    refineEdges: true,
+                    numIterations: 1,
+                    decisionMargin: 0,
+                    hammingDist: 0,
                 }
             }
         ],

photon-client/src/views/CamerasView.vue

@@ -31,14 +31,6 @@
               :label-cols="$vuetify.breakpoint.mdAndUp ? undefined : 7"
             />
             <br>
-            <CVnumberinput
-              v-model="cameraSettings.tiltDegrees"
-              name="Camera pitch"
-              tooltip="How many degrees above the horizontal the physical camera is tilted"
-              :step="0.01"
-              :label-cols="$vuetify.breakpoint.mdAndUp ? undefined : 7"
-            />
-            <br>
             <v-btn
               style="margin-top:10px"
               small
@@ -146,6 +138,24 @@
                             text="Standard Deviation"
                           />
                         </th>
+                        <th class="text-center">
+                          <tooltipped-label
+                            tooltip="Estimated Horizontal FOV, in degrees"
+                            text="Horizontal FOV"
+                          />
+                        </th>
+                        <th class="text-center">
+                          <tooltipped-label
+                            tooltip="Estimated Vertical FOV, in degrees"
+                            text="Vertical FOV"
+                          />
+                        </th>
+                        <th class="text-center">
+                          <tooltipped-label
+                            tooltip="Estimated Diagonal FOV, in degrees"
+                            text="Diagonal FOV"
+                          />
+                        </th>
                       </tr>
                     </thead>
                     <tbody>
@@ -158,6 +168,9 @@
                           {{ isCalibrated(value) ? value.mean.toFixed(2) + "px" : "—" }}
                         </td>
                         <td> {{ isCalibrated(value) ? value.standardDeviation.toFixed(2) + "px" : "—" }} </td>
+                        <td> {{ isCalibrated(value) ? value.horizontalFOV.toFixed(2) + "°" : "—" }} </td>
+                        <td> {{ isCalibrated(value) ? value.verticalFOV.toFixed(2) + "°" : "—" }} </td>
+                        <td> {{ isCalibrated(value) ? value.diagonalFOV.toFixed(2) + "°" : "—" }} </td>
                       </tr>
                     </tbody>
                   </v-simple-table>
@@ -181,10 +194,13 @@
               >
                 <CVslider
                   v-model="$store.getters.currentPipelineSettings.cameraExposure"
+                  :disabled="$store.getters.currentPipelineSettings.cameraAutoExposure"
                   name="Exposure"
                   :min="0"
                   :max="100"
                   slider-cols="8"
+                  step="0.1"
+                  tooltip="Directly controls how much light is allowed to fall onto the sensor, which affects apparent brightness"
                   @input="e => handlePipelineUpdate('cameraExposure', e)"
                 />
                 <CVslider
@@ -195,6 +211,13 @@
                   slider-cols="8"
                   @input="e => handlePipelineUpdate('cameraBrightness', e)"
                 />
+                <CVswitch
+                  v-model="$store.getters.currentPipelineSettings.cameraAutoExposure"
+                  class="pt-2"
+                  name="Auto Exposure"
+                  tooltip="Enables or Disables camera automatic adjustment for current lighting conditions"
+                  @input="e => handlePipelineUpdate('cameraAutoExposure', e)"
+                />
                 <CVslider
                   v-if="$store.getters.currentPipelineSettings.cameraRedGain !== -1"
                   v-model="$store.getters.currentPipelineSettings.cameraRedGain"
@@ -268,7 +291,8 @@
       >
         <template>
           <CVimage
-            :address="$store.getters.streamAddress[1]"
+            :id="cameras-cal"
+            :idx=1
             :disconnected="!$store.state.backendConnected"
             scale="100"
             style="border-radius: 5px;"
@@ -339,6 +363,7 @@
 import CVselect from '../components/common/cv-select';
 import CVnumberinput from '../components/common/cv-number-input';
 import CVslider from '../components/common/cv-slider';
+import CVswitch from '../components/common/cv-switch';
 import CVimage from "../components/common/cv-image";
 import TooltippedLabel from "../components/common/cv-tooltipped-label";
 import jsPDF from "jspdf";
@@ -351,6 +376,7 @@ export default {
         CVselect,
         CVnumberinput,
         CVslider,
+        CVswitch,
         CVimage
     },
     data() {
@@ -396,6 +422,9 @@ export default {
                         if (calib != null) {
                             it['standardDeviation'] = calib.standardDeviation;
                             it['mean'] = calib.perViewErrors.reduce((a, b) => a + b) / calib.perViewErrors.length;
+                            it['horizontalFOV'] = 2 * Math.atan2(it.width/2,calib.intrinsics[0]) * (180/Math.PI);
+                            it['verticalFOV'] = 2 * Math.atan2(it.height/2,calib.intrinsics[4]) * (180/Math.PI);
+                            it['diagonalFOV'] = 2 * Math.atan2(Math.sqrt(it.width**2 + (it.height/(calib.intrinsics[4]/calib.intrinsics[0]))**2)/2,calib.intrinsics[0]) * (180/Math.PI);
                         }
                         filtered.push(it);
                     }
@@ -404,13 +433,11 @@ export default {
                 return filtered
             }
         },
-
         stringResolutionList: {
             get() {
                 return this.filteredResolutionList.map(res => `${res['width']} X ${res['height']}`);
             }
         },
-
         cameraSettings: {
             get() {
                 return this.$store.getters.currentCameraSettings;
@@ -419,7 +446,6 @@ export default {
                 this.$store.commit('cameraSettings', value);
             }
         },
-
         boardType: {
             get() {
                 return this.calibrationData.boardType
@@ -601,8 +627,7 @@ export default {
             this.axios.post("http://" + this.$address + "/api/settings/camera", {
                 "settings": this.cameraSettings,
                 "index": this.$store.state.currentCameraIndex
-            }).then(
-                function (response) {
+            }).then(response => {
                     if (response.status === 200) {
                         this.$store.state.saveBar = true;
                     }
@@ -623,13 +648,14 @@ export default {
             if (this.isCalibrating === true) {
                 data['takeCalibrationSnapshot'] = true
             } else {
+                // This store prevents an edge case of a user not selecting a different resolution, which causes the set logic to not be called
+                this.$store.commit('mutateCalibrationState', {['videoModeIndex']: this.filteredResolutionList[this.selectedFilteredResIndex].index});
                 const calData = this.calibrationData;
                 calData.isCalibrating = true;
                 data['startPnpCalibration'] = calData;
-
                 console.log("starting calibration with index " + calData.videoModeIndex);
             }
-
+            this.$store.commit('currentPipelineIndex', -2);
             this.$socket.send(this.$msgPack.encode(data));
         },
         sendCalibrationFinish() {

photon-client/src/views/PipelineView.vue

@@ -36,7 +36,7 @@
                 <span class="pr-1">{{ Math.round($store.state.pipelineResults.fps) }}&nbsp;FPS &ndash;</span>
                 <span v-if="!fpsTooLow">{{ Math.min(Math.round($store.state.pipelineResults.latency), 9999) }} ms latency</span>
                 <span v-else-if="!$store.getters.currentPipelineSettings.inputShouldShow">HSV thresholds are too broad; narrow them for better performance</span>
-                <span v-else>stop viewing the color stream for better performance</span>
+                <span v-else>stop viewing the raw stream for better performance</span>
               </v-chip>
               <v-switch
                 v-model="driverMode"
@@ -58,16 +58,16 @@
               >
                 <div style="position: relative; width: 100%; height: 100%;">
                   <cv-image
-                    :id="idx === 0 ? 'normal-stream' : ''"
+                    :id="idx === 0 ? 'raw-stream' : 'processed-stream'"
                     ref="streams"
-                    :address="$store.getters.streamAddress[idx]"
+                    :idx=idx
                     :disconnected="!$store.state.backendConnected"
                     scale="100"
                     :max-height="$store.getters.isDriverMode ? '40vh' : '300px'"
                     :max-height-md="$store.getters.isDriverMode ? '50vh' : '380px'"
                     :max-height-lg="$store.getters.isDriverMode ? '55vh' : '390px'"
                     :max-height-xl="$store.getters.isDriverMode ? '60vh' : '450px'"
-                    :alt="'Stream' + idx"
+                    :alt="'Stream ' + idx"
                     :color-picking="$store.state.colorPicking && idx === 0"
                     @click="onImageClick"
                   />
@@ -85,7 +85,7 @@
           <v-card
             color="primary"
           >
-            <camera-and-pipeline-select @camera-name-changed="reloadStreams" />
+            <camera-and-pipeline-select />
           </v-card>
           <v-card
             :disabled="$store.getters.isDriverMode || $store.state.colorPicking"
@@ -136,15 +136,15 @@
                     color="secondary"
                     class="fill"
                   >
-                    <v-icon>mdi-palette</v-icon>
-                    <span>Normal</span>
+                    <v-icon>mdi-import</v-icon>
+                    <span>Raw</span>
                   </v-btn>
                   <v-btn
                     color="secondary"
                     class="fill"
                   >
-                    <v-icon>mdi-compare</v-icon>
-                    <span>Threshold</span>
+                    <v-icon>mdi-export</v-icon>
+                    <span>Processed</span>
                   </v-btn>
                 </v-btn-toggle>
               </v-col>
@@ -261,6 +261,7 @@ import ThresholdTab from './PipelineViews/ThresholdTab';
 import ContoursTab from './PipelineViews/ContoursTab';
 import OutputTab from './PipelineViews/OutputTab';
 import TargetsTab from "./PipelineViews/TargetsTab";
+import Map3DTab from './PipelineViews/Map3DTab';
 import PnPTab from './PipelineViews/PnPTab';
 import AprilTagTab from './PipelineViews/AprilTagTab';
 
@@ -274,6 +275,7 @@ export default {
         ContoursTab,
         OutputTab,
         TargetsTab,
+        Map3DTab,
         PnPTab,
         AprilTagTab,
     },
@@ -319,12 +321,16 @@ export default {
                         component: "OutputTab",
                     },
                     targets: {
-                        name: "Target Info",
+                        name: "Targets",
                         component: "TargetsTab",
                     },
                     pnp: {
-                        name: "3D",
+                        name: "PnP",
                         component: "PnPTab",
+                    },
+                    map3d: {
+                        name: "3D",
+                        component: "Map3DTab",
                     }
                 };
 
@@ -341,18 +347,18 @@ export default {
                 } else if (this.$vuetify.breakpoint.mdAndDown || !this.$store.state.compactMode) {
                     // Two tab groups, one with "input, threshold, contours, output" and the other with "target info, 3D"
                     ret[0] = [tabs.input, tabs.threshold, tabs.contours, tabs.apriltag, tabs.output];
-                    ret[1] = [tabs.targets, tabs.pnp];
+                    ret[1] = [tabs.targets, tabs.pnp, tabs.map3d];
                 } else if (this.$vuetify.breakpoint.lgAndDown) {
                     // Three tab groups, one with "input", one with "threshold, contours, output", and the other with "target info, 3D"
                     ret[0] = [tabs.input];
                     ret[1] = [tabs.threshold, tabs.contours, tabs.apriltag, tabs.output];
-                    ret[2] = [tabs.targets, tabs.pnp];
+                    ret[2] = [tabs.targets, tabs.pnp, tabs.map3d];
                 } else if (this.$vuetify.breakpoint.xl) {
                     // Three tab groups, one with "input", one with "threshold, contours", and the other with "output, target info, 3D"
                     ret[0] = [tabs.input];
                     ret[1] = [tabs.threshold];
                     ret[2] = [tabs.contours, tabs.apriltag, tabs.output];
-                    ret[3] = [tabs.targets, tabs.pnp];
+                    ret[3] = [tabs.targets, tabs.pnp, tabs.map3d];
                 }
 
                 for(let i = 0; i < ret.length; i++) {
@@ -360,10 +366,11 @@ export default {
 
                   // All the tabs we allow
                   const filteredGroup = group.filter(it =>
-                          !(!allow3d && it.name === "3D")
-                       && !(isAprilTag && (it.name === "Threshold"))
-                       && !(isAprilTag && (it.name === "Contours"))
-                       && !(!isAprilTag && it.name === "AprilTag")
+                          !(!allow3d && it.name === "3D") //Filter out 3D tab any time 3D isn't calibrated
+                       && !((!allow3d || isAprilTag) && it.name === "PnP") //Filter out the PnP config tab if 3D isn't available, or we're doing Apriltags
+                       && !(isAprilTag && (it.name === "Threshold")) //Filter out threshold tab if we're doing apriltags
+                       && !(isAprilTag && (it.name === "Contours")) //Filter out contours if we're doing Apriltag
+                       && !(!isAprilTag && it.name === "AprilTag") //Filter out apriltag unless we actually are doing Apriltags
                       );
                   ret[i] = filteredGroup;
                 }

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

@@ -7,45 +7,93 @@
       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="12"
+      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')"
     />
     <CVslider
       v-model="blur"
       class="pt-2"
-      slider-cols="12"
+      slider-cols="8"
       name="Blur"
       min="0"
       max="5"
       step=".01"
+      tooltip="Gaussian blur added to the image, high FPS cost for slightly decreased noise"
       @input="handlePipelineData('blur')"
     />
     <CVslider
       v-model="threads"
       class="pt-2"
-      slider-cols="12"
+      slider-cols="8"
       name="Threads"
       min="1"
       max="8"
       step="1"
+      tooltip="Number of threads spawned by the AprilTag detector"
       @input="handlePipelineData('threads')"
     />
     <CVswitch
       v-model="refineEdges"
       class="pt-2"
-      slider-cols="12"
+      slider-cols="8"
       name="Refine Edges"
+      tooltip="Further refines the apriltag corner position initial estimate, suggested left on"
       @input="handlePipelineData('refineEdges')"
     />
+    <CVslider
+      v-model="hammingDist"
+      class="pt-2 pb-4"
+      slider-cols="8"
+      name="Max error bits"
+      min="0"
+      max="10"
+      step="1"
+      tooltip="Maximum number of error bits to correct; potential tags with more will be thrown out. For smaller tags (like 16h5), set this as low as possible."
+      @input="handlePipelineData('hammingDist')"
+    />
+    <CVslider
+      v-model="decisionMargin"
+      class="pt-2 pb-4"
+      slider-cols="8"
+      name="Decision Margin Cutoff"
+      min="0"
+      max="250"
+      step="1"
+      tooltip="Tags with a 'margin' (decoding quality score) less than this wil be rejected. Increase this to reduce the number of false positive detections"
+      @input="handlePipelineData('decisionMargin')"
+    />
+    <CVslider
+      v-model="numIterations"
+      class="pt-2 pb-4"
+      slider-cols="8"
+      name="Pose Estimation Iterations"
+      min="0"
+      max="500"
+      step="1"
+      tooltip="Number of iterations the pose estimation algorithm will run, 50-100 is a good starting point"
+      @input="handlePipelineData('numIterations')"
+    />
   </div>
 </template>
 
@@ -61,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
@@ -82,6 +141,30 @@
                 this.$store.commit("mutatePipeline", {"decimate": val});
             }
           },
+          hammingDist: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.hammingDist
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"hammingDist": val});
+            }
+          },
+          decisionMargin: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.decisionMargin
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"decisionMargin": val});
+            }
+          },
+          numIterations: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.numIterations
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"numIterations": val});
+            }
+          },
           blur: {
             get() {
                 return this.$store.getters.currentPipelineSettings.blur

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

@@ -19,12 +19,12 @@
       @input="handlePipelineData('contourRatio')"
     />
     <CVselect
-        v-model="contourTargetOrientation"
-        name="Target Orientation"
-        tooltip="Used to determine how to calculate target landmarks, as well as aspect ratio"
-        :list="['Portrait', 'Landscape']"
-        @input="handlePipelineData('contourTargetOrientation')"
-        @rollback="e=> rollback('contourTargetOrientation', e)"
+      v-model="contourTargetOrientation"
+      name="Target Orientation"
+      tooltip="Used to determine how to calculate target landmarks, as well as aspect ratio"
+      :list="['Portrait', 'Landscape']"
+      @input="handlePipelineData('contourTargetOrientation')"
+      @rollback="e=> rollback('contourTargetOrientation', e)"
     />
     <CVrangeSlider
       v-if="currentPipelineType() !== 3"

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

@@ -1,13 +1,13 @@
 <template>
   <div>
     <CVslider
-      v-if="cameraExposure !== -1"
       v-model="cameraExposure"
+      :disabled="cameraAutoExposure"
       name="Exposure"
       min="0"
       max="100"
       step="0.1"
-      tooltip="Directly controls how much light is allowed to fall onto the sensor, which affects brightness"
+      tooltip="Directly controls how much light is allowed to fall onto the sensor, which affects apparent brightness"
       :slider-cols="largeBox"
       @input="handlePipelineData('cameraExposure')"
       @rollback="e => rollback('cameraExposure', e)"
@@ -22,10 +22,28 @@
       @input="handlePipelineData('cameraBrightness')"
       @rollback="e => rollback('cameraBrightness', e)"
     />
+    <CVswitch
+      v-model="cameraAutoExposure"
+      class="pt-2"
+      name="Auto Exposure"
+      tooltip="Enables or Disables camera automatic adjustment for current lighting conditions"
+      @input="handlePipelineData('cameraAutoExposure')"
+    />
+    <CVslider
+      v-if="cameraGain >= 0"
+      v-model="cameraGain"
+      name="Camera Gain"
+      min="0"
+      max="100"
+      tooltip="Controls camera gain, similar to brightness"
+      :slider-cols="largeBox"
+      @input="handlePipelineData('cameraGain')"
+      @rollback="e => rollback('cameraGain', e)"
+    />
     <CVslider
       v-if="cameraRedGain !== -1"
       v-model="cameraRedGain"
-      name="Red AWB Gain"
+      name="Red Balance"
       min="0"
       max="100"
       tooltip="Controls red automatic white balance gain, which affects how the camera captures colors in different conditions"
@@ -36,7 +54,7 @@
     <CVslider
       v-if="cameraBlueGain !== -1"
       v-model="cameraBlueGain"
-      name="Blue AWB Gain"
+      name="Blue Balance"
       min="0"
       max="100"
       tooltip="Controls blue automatic white balance gain, which affects how the camera captures colors in different conditions"
@@ -76,6 +94,7 @@
 <script>
     import CVslider from '../../components/common/cv-slider'
     import CVselect from '../../components/common/cv-select'
+    import CVswitch from '../../components/common/cv-switch'
 
     const unfilteredStreamDivisors = [1, 2, 4, 6];
 
@@ -84,14 +103,10 @@
         components: {
             CVslider,
             CVselect,
+            CVswitch,
         },
         // eslint-disable-next-line vue/require-prop-types
         props: ['value'],
-        data() {
-            return {
-              rawStreamDivisorIndex: 0,
-            }
-        },
         computed: {
             largeBox: {
               get() {
@@ -109,6 +124,14 @@
                     this.$store.commit("mutatePipeline", {"cameraExposure": parseFloat(val)});
                 }
             },
+            cameraAutoExposure: {
+                get() {
+                    return this.$store.getters.currentPipelineSettings.cameraAutoExposure;
+                },
+                set(val) {
+                    this.$store.commit("mutatePipeline", {"cameraAutoExposure": val});
+                }
+            },
             cameraBrightness: {
                 get() {
                     return parseInt(this.$store.getters.currentPipelineSettings.cameraBrightness)
@@ -117,6 +140,14 @@
                     this.$store.commit("mutatePipeline", {"cameraBrightness": parseInt(val)});
                 }
             },
+            cameraGain: {
+                get() {
+                    return parseInt(this.$store.getters.currentPipelineSettings.cameraGain)
+                },
+                set(val) {
+                    this.$store.commit("mutatePipeline", {"cameraGain": parseInt(val)});
+                }
+            },
             cameraRedGain: {
                 get() {
                     return parseInt(this.$store.getters.currentPipelineSettings.cameraRedGain)
@@ -149,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);
                 }
             },

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

@@ -0,0 +1,53 @@
+<template>
+  <div>
+    <mini-map
+      class="miniMapClass"
+      :targets="targets"
+      :horizontal-f-o-v="horizontalFOV"
+    />
+  </div>
+</template>
+
+<script>
+    import miniMap from '../../components/pipeline/3D/MiniMap';
+
+    export default {
+        name: "Map3D",
+        components: {
+            miniMap
+        },
+        data() {
+            return {
+            }
+        },
+        computed: {
+            targets: {
+                get() {
+                    return this.$store.getters.currentPipelineResults.targets;
+                }
+            },
+            horizontalFOV: {
+                get() {
+                    let index = this.$store.getters.currentPipelineSettings.cameraVideoModeIndex;
+                    let FOV = this.$store.getters.currentCameraSettings.fov;
+                    let resolution = this.$store.getters.videoFormatList[index];
+                    let diagonalView = FOV * (Math.PI / 180);
+                    let diagonalAspect = Math.hypot(resolution.width, resolution.height);
+                    return Math.atan(Math.tan(diagonalView / 2) * (resolution.width / diagonalAspect)) * 2 * (180 / Math.PI)
+                }
+            },
+        },
+        methods: {
+        }
+    }
+</script>
+
+<style scoped>
+    .miniMapClass {
+        width: 400px !important;
+        height: 100% !important;
+
+        margin-left: auto;
+        margin-right: auto;
+    }
+</style>

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

@@ -6,7 +6,6 @@
       type="file"
       accept=".csv"
       style="display: none;"
-
       @change="readFile"
     >
 
@@ -32,11 +31,7 @@
       @input="handlePipelineData('cornerDetectionAccuracyPercentage')"
       @rollback="e => rollback('cornerDetectionAccuracyPercentage', e)"
     />
-    <mini-map
-      class="miniMapClass"
-      :targets="targets"
-      :horizontal-f-o-v="horizontalFOV"
-    />
+
     <v-snackbar
       v-model="snack"
       top
@@ -49,18 +44,16 @@
 
 <script>
     import Papa from 'papaparse';
-    import miniMap from '../../components/pipeline/3D/MiniMap';
     import CVslider from '../../components/common/cv-slider'
 
     export default {
         name: "PnP",
         components: {
-            CVslider,
-            miniMap
+            CVslider
         },
         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: ""
@@ -72,7 +65,6 @@
             selectedModel: {
                 get() {
                     let ret = this.$store.getters.currentPipelineSettings.targetModel
-                    console.log(ret)
                     return this.targetList[ret];
                 },
                 set(val) {
@@ -87,21 +79,6 @@
                     this.$store.commit("mutatePipeline", {"cornerDetectionAccuracyPercentage": val});
                 }
             },
-            targets: {
-                get() {
-                    return this.$store.getters.currentPipelineResults.targets;
-                }
-            },
-            horizontalFOV: {
-                get() {
-                    let index = this.$store.getters.currentPipelineSettings.cameraVideoModeIndex;
-                    let FOV = this.$store.getters.currentCameraSettings.fov;
-                    let resolution = this.$store.getters.videoFormatList[index];
-                    let diagonalView = FOV * (Math.PI / 180);
-                    let diagonalAspect = Math.hypot(resolution.width, resolution.height);
-                    return Math.atan(Math.tan(diagonalView / 2) * (resolution.width / diagonalAspect)) * 2 * (180 / Math.PI)
-                }
-            },
         },
         methods: {
             readFile(event) {

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

@@ -18,7 +18,10 @@
               <th class="text-center">
                 Target
               </th>
-              <th class="text-center" v-if="$store.getters.pipelineType === 4">
+              <th
+                v-if="$store.getters.pipelineType === 4"
+                class="text-center"
+              >
                 Fiducial ID
               </th>
               <template v-if="!$store.getters.currentPipelineSettings.solvePNPEnabled">
@@ -46,6 +49,11 @@
                   Z Angle,&nbsp;&deg;
                 </th>
               </template>
+              <template v-if="$store.getters.pipelineType === 4 && $store.getters.currentPipelineSettings.solvePNPEnabled">
+                <th class="text-center">
+                  Ambiguity
+                </th>
+              </template>
             </tr>
           </thead>
           <tbody>
@@ -73,6 +81,11 @@
                 <td>{{ parseFloat(value.pose.y).toFixed(2) }}&nbsp;m</td>
                 <td>{{ (parseFloat(value.pose.angle_z) * 180 / Math.PI).toFixed(2) }}&deg;</td>
               </template>
+              <template v-if="$store.getters.pipelineType === 4 && $store.getters.currentPipelineSettings.solvePNPEnabled">
+                <td>
+                  {{ parseFloat(value.ambiguity).toFixed(2) }}
+                </td>
+              </template>
             </tr>
           </tbody>
         </template>

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

@@ -49,22 +49,46 @@
           <th class="infoElem">
             Disk Usage
           </th>
+          <th class="infoElem">
+            <v-tooltip top>
+              <template v-slot:activator="{ on, attrs }">
+                <span
+                  v-bind="attrs"
+                  v-on="on"
+                >
+                ⓘ CPU Throttling
+                </span>
+              </template>
+              <span>
+                    Current or Previous Reason for the cpu being held back from maximum performance.
+              </span>
+            </v-tooltip>
+          </th>
+          <th class="infoElem">
+            CPU Uptime
+          </th>
         </tr>
         <tr v-if="metrics.cpuUtil !== 'N/A'">
           <td class="infoElem">
-            {{ metrics.cpuUtil.replace(" ", "") }}%
+            {{ metrics.cpuUtil }}%
           </td>
           <td class="infoElem">
             {{ parseInt(metrics.cpuTemp) }}&deg;&nbsp;C
           </td>
           <td class="infoElem">
-            {{ metrics.ramUtil.replace(" ", "") }}MB of {{ metrics.cpuMem }}MB
+            {{ metrics.ramUtil }}MB of {{ metrics.cpuMem }}MB
           </td>
           <td class="infoElem">
-            {{ metrics.gpuMemUtil.replace(" ", "") }}MB of {{ metrics.gpuMem }}MB
+            {{ metrics.gpuMemUtil }}MB of {{ metrics.gpuMem }}MB
           </td>
           <td class="infoElem">
-            {{ metrics.diskUtilPct.replace(" ", "") }}
+            {{ metrics.diskUtilPct }}
+          </td>
+          <td class="infoElem">
+            {{ metrics.cpuThr }}
+          </td>
+          <td class="infoElem">
+            {{ metrics.cpuUptime }}
           </td>
         </tr>
         <tr v-if="metrics.cpuUtil === 'N/A'">
@@ -83,6 +107,12 @@
           <td class="infoElem">
             ---
           </td>
+          <td class="infoElem">
+            ---
+          </td>
+          <td class="infoElem">
+            ---
+          </td>
         </tr>
       </table>
     </v-row>

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

@@ -66,7 +66,16 @@
     >
       Save
     </v-btn>
+    <v-snackbar
+      v-model="snack"
+      top
+      :color="snackbar.color"
+      timeout="5000"
+    >
+      <span>{{ snackbar.text }}</span>
+    </v-snackbar>
     <v-divider class="mt-4 mb-4" />
+    <!-- TEMP - RIO finder is not currently enabled
     <v-row>
       <v-col
         cols="12"
@@ -125,6 +134,7 @@
         </v-simple-table>
       </v-col>
     </v-row>
+    -->
   </div>
 </template>
 
@@ -237,7 +247,7 @@ export default {
         },
         sendGeneralSettings() {
             this.axios.post("http://" + this.$address + "/api/settings/general", this.settings).then(
-                function (response) {
+                response => {
                     if (response.status === 200) {
                         this.snackbar = {
                             color: "success",
@@ -246,7 +256,7 @@ export default {
                         this.snack = true;
                     }
                 },
-                function (error) {
+                error => {
                     this.snackbar = {
                         color: "error",
                         text: (error.response || {data: "Couldn't save settings"}).data

photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java

@@ -72,6 +72,7 @@ public class CameraConfiguration {
         logger.debug(
                 "Creating USB camera configuration for "
                         + cameraType
+                        + " "
                         + baseName
                         + " (AKA "
                         + nickname
@@ -101,6 +102,7 @@ public class CameraConfiguration {
         logger.debug(
                 "Creating camera configuration for "
                         + cameraType
+                        + " "
                         + baseName
                         + " (AKA "
                         + nickname

photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java

@@ -438,7 +438,7 @@ public class ConfigManager {
             try {
                 Thread.sleep(1000);
             } catch (InterruptedException e) {
-                logger.error("Exception waiting for settings semaphor", e);
+                logger.error("Exception waiting for settings semaphore", e);
             }
         }
     }

photon-core/src/main/java/org/photonvision/common/configuration/HardwareConfig.java

@@ -41,6 +41,8 @@ public class HardwareConfig {
     public final String cpuTempCommand;
     public final String cpuMemoryCommand;
     public final String cpuUtilCommand;
+    public final String cpuThrottleReasonCmd;
+    public final String cpuUptimeCommand;
     public final String gpuMemoryCommand;
     public final String ramUtilCommand;
     public final String gpuMemUsageCommand;
@@ -65,6 +67,8 @@ public class HardwareConfig {
         cpuTempCommand = "";
         cpuMemoryCommand = "";
         cpuUtilCommand = "";
+        cpuThrottleReasonCmd = "";
+        cpuUptimeCommand = "";
         gpuMemoryCommand = "";
         ramUtilCommand = "";
         ledBlinkCommand = "";
@@ -91,6 +95,8 @@ public class HardwareConfig {
             String cpuTempCommand,
             String cpuMemoryCommand,
             String cpuUtilCommand,
+            String cpuThrottleReasonCmd,
+            String cpuUptimeCommand,
             String gpuMemoryCommand,
             String ramUtilCommand,
             String gpuMemUsageCommand,
@@ -111,6 +117,8 @@ public class HardwareConfig {
         this.cpuTempCommand = cpuTempCommand;
         this.cpuMemoryCommand = cpuMemoryCommand;
         this.cpuUtilCommand = cpuUtilCommand;
+        this.cpuThrottleReasonCmd = cpuThrottleReasonCmd;
+        this.cpuUptimeCommand = cpuUptimeCommand;
         this.gpuMemoryCommand = gpuMemoryCommand;
         this.ramUtilCommand = ramUtilCommand;
         this.gpuMemUsageCommand = gpuMemUsageCommand;

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

@@ -189,7 +189,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
             targetAreaEntry.forceSetDouble(bestTarget.getArea());
             targetSkewEntry.forceSetDouble(bestTarget.getSkew());
 
-            var pose = bestTarget.getCameraToTarget3d();
+            var pose = bestTarget.getBestCameraToTarget3d();
             targetPoseEntry.forceSetDoubleArray(
                     new double[] {
                         pose.getTranslation().getX(),
@@ -232,7 +232,9 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
                             t.getArea(),
                             t.getSkew(),
                             t.getFiducialId(),
-                            t.getCameraToTarget3d(),
+                            t.getBestCameraToTarget3d(),
+                            t.getAltCameraToTarget3d(),
+                            t.getPoseAmbiguity(),
                             cornerList));
         }
         return ret;

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

@@ -23,6 +23,7 @@ import edu.wpi.first.networktables.NetworkTableInstance;
 import java.util.HashMap;
 import java.util.function.Consumer;
 import org.photonvision.PhotonVersion;
+import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.configuration.NetworkConfig;
 import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
@@ -37,8 +38,11 @@ public class NetworkTablesManager {
     private final String kRootTableName = "/photonvision";
     public final NetworkTable kRootTable = ntInstance.getTable(kRootTableName);
 
+    private boolean isRetryingConnection = false;
+
     private NetworkTablesManager() {
         ntInstance.addLogger(new NTLogger(), 0, 255); // to hide error messages
+        TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
     }
 
     private static NetworkTablesManager INSTANCE;
@@ -109,17 +113,11 @@ public class NetworkTablesManager {
     }
 
     private void setClientMode(int teamNumber) {
-        logger.info("Starting NT Client");
+        if (!isRetryingConnection) logger.info("Starting NT Client");
         ntInstance.stopServer();
 
         ntInstance.startClientTeam(teamNumber);
         ntInstance.startDSClient();
-        if (ntInstance.isConnected()) {
-            logger.info("[NetworkTablesManager] Connected to the robot!");
-        } else {
-            logger.error(
-                    "[NetworkTablesManager] Could not connect to the robot! Will retry in the background...");
-        }
         broadcastVersion();
     }
 
@@ -129,4 +127,22 @@ public class NetworkTablesManager {
         ntInstance.startServer();
         broadcastVersion();
     }
+
+    // So it seems like if Photon starts before the robot NT server does, and both aren't static IP,
+    // it'll never connect. This hack works around it by restarting the client/server while the nt
+    // instance
+    // isn't connected, same as clicking the save button in the settings menu (or restarting the
+    // service)
+    private void ntTick() {
+        if (!ntInstance.isConnected()
+                && !ConfigManager.getInstance().getConfig().getNetworkConfig().runNTServer) {
+            setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig());
+        }
+
+        if (!ntInstance.isConnected() && !isRetryingConnection) {
+            isRetryingConnection = true;
+            logger.error(
+                    "[NetworkTablesManager] Could not connect to the robot! Will retry in the background...");
+        }
+    }
 }

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

@@ -85,6 +85,8 @@ public class VisionLED {
                 pigpioSocket.generateAndSendWaveform(pulseLengthMillis, blinkCount, ledPins);
             } catch (PigpioException e) {
                 logger.error("Failed to blink!", e);
+            } catch (NullPointerException e) {
+                logger.error("Failed to blink, pigpio internal issue!", e);
             }
         } else {
             for (GPIOBase led : visionLEDs) {
@@ -100,13 +102,19 @@ public class VisionLED {
                 pigpioSocket.waveTxStop();
             } catch (PigpioException e) {
                 logger.error("Failed to stop blink!", e);
+            } catch (NullPointerException e) {
+                logger.error("Failed to blink, pigpio internal issue!", e);
             }
         }
-        // if the user has set an LED brightness other than 100%, use that instead
-        if (mappedBrightnessPercentage == 100 || !state) {
-            visionLEDs.forEach((led) -> led.setState(state));
-        } else {
-            visionLEDs.forEach((led) -> led.setBrightness(mappedBrightnessPercentage));
+        try {
+            // if the user has set an LED brightness other than 100%, use that instead
+            if (mappedBrightnessPercentage == 100 || !state) {
+                visionLEDs.forEach((led) -> led.setState(state));
+            } else {
+                visionLEDs.forEach((led) -> led.setBrightness(mappedBrightnessPercentage));
+            }
+        } catch (NullPointerException e) {
+            logger.error("Failed to blink, pigpio internal issue!", e);
         }
     }
 

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

@@ -40,4 +40,12 @@ public class CPUMetrics extends MetricsBase {
     public String getUtilization() {
         return execute(cpuUtilizationCommand);
     }
+
+    public String getUptime() {
+        return execute(cpuUptimeCommand);
+    }
+
+    public String getThrottleReason() {
+        return execute(cpuThrottleReasonCmd);
+    }
 }

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

@@ -26,7 +26,7 @@ import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.ShellExec;
 
 public abstract class MetricsBase {
-    private static final Logger logger = new Logger(MetricsBase.class, LogGroup.General);
+    static final Logger logger = new Logger(MetricsBase.class, LogGroup.General);
     // CPU
     public static String cpuMemoryCommand = "vcgencmd get_mem arm | grep -Eo '[0-9]+'";
     public static String cpuTemperatureCommand =
@@ -34,6 +34,15 @@ public abstract class MetricsBase {
     public static String cpuUtilizationCommand =
             "top -bn1 | grep \"Cpu(s)\" | sed \"s/.*, *\\([0-9.]*\\)%* id.*/\\1/\" | awk '{print 100 - $1}'";
 
+    public static String cpuThrottleReasonCmd =
+            "if ((  $(( $(vcgencmd get_throttled | grep -Eo 0x[0-9a-fA-F]*) & 0x01 )) != 0x00 )); then echo \"LOW VOLTAGE\"; "
+                    + "elif ((  $(( $(vcgencmd get_throttled | grep -Eo 0x[0-9a-fA-F]*) & 0x08 )) != 0x00 )); then echo \"HIGH TEMP\"; "
+                    + "elif ((  $(( $(vcgencmd get_throttled | grep -Eo 0x[0-9a-fA-F]*) & 0x10000 )) != 0x00 )); then echo \"Prev. Low Voltage\"; "
+                    + "elif ((  $(( $(vcgencmd get_throttled | grep -Eo 0x[0-9a-fA-F]*) & 0x80000 )) != 0x00 )); then echo \"Prev. High Temp\"; "
+                    + " else echo \"None\"; fi";
+
+    public static String cpuUptimeCommand = "uptime -p | cut -c 4-";
+
     // GPU
     public static String gpuMemoryCommand = "vcgencmd get_mem gpu | grep -Eo '[0-9]+'";
     public static String gpuMemUsageCommand = "vcgencmd get_mem malloc | grep -Eo '[0-9]+'";
@@ -51,6 +60,8 @@ public abstract class MetricsBase {
         cpuMemoryCommand = config.cpuMemoryCommand;
         cpuTemperatureCommand = config.cpuTempCommand;
         cpuUtilizationCommand = config.cpuUtilCommand;
+        cpuThrottleReasonCmd = config.cpuThrottleReasonCmd;
+        cpuUptimeCommand = config.cpuUptimeCommand;
 
         gpuMemoryCommand = config.gpuMemoryCommand;
         gpuMemUsageCommand = config.gpuMemUsageCommand;

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

@@ -60,6 +60,8 @@ public class MetricsPublisher {
         metrics.put("cpuTemp", cpuMetrics.getTemp());
         metrics.put("cpuUtil", cpuMetrics.getUtilization());
         metrics.put("cpuMem", cpuMetrics.getMemory());
+        metrics.put("cpuThr", cpuMetrics.getThrottleReason());
+        metrics.put("cpuUptime", cpuMetrics.getUptime());
         metrics.put("gpuMem", gpuMetrics.getGPUMemorySplit());
         metrics.put("ramUtil", ramMetrics.getUsedRam());
         metrics.put("gpuMemUtil", gpuMetrics.getMallocedMemory());

photon-core/src/main/java/org/photonvision/common/util/TestUtils.java

@@ -33,7 +33,6 @@ public class TestUtils {
         try {
             CameraServerCvJNI.forceLoad();
             //        PicamJNI.forceLoad();
-            // AprilTagJNI.forceLoad();
         } catch (IOException ex) {
             // ignored
         }
@@ -165,7 +164,8 @@ public class TestUtils {
     }
 
     public enum ApriltagTestImages {
-        kRobots;
+        kRobots,
+        kTag1_640_480;
 
         public final Path path;
 
@@ -194,7 +194,7 @@ public class TestUtils {
     public static Path getTestMode2020ImagePath() {
         return getResourcesFolderPath(true)
                 .resolve("testimages")
-                .resolve(WPI2020Image.kBlueGoal_108in_Center.path);
+                .resolve(WPI2020Image.kBlueGoal_156in_Left.path);
     }
 
     public static Path getTestMode2022ImagePath() {
@@ -233,6 +233,10 @@ public class TestUtils {
         return getTestImagesPath(testMode).resolve(image.path);
     }
 
+    public static Path getApriltagImagePath(ApriltagTestImages image, boolean testMode) {
+        return getTestImagesPath(testMode).resolve(image.path);
+    }
+
     public static Path getPowercellImagePath(PowercellTestImages image, boolean testMode) {
         return getPowercellPath(testMode).resolve(image.path);
     }

photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java

@@ -18,15 +18,23 @@
 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;
 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 {
     MathUtils() {}
@@ -156,28 +164,77 @@ public class MathUtils {
                 pose.getTranslation().rotateBy(rotationQuat), pose.getRotation().rotateBy(rotationQuat));
     }
 
-    // TODO: Refactor into new pipe?
-    public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
-        // CameraToTarget _should_ be in opencv-land EDN
+    /**
+     * All our solvepnp code returns a tag with X left, Y up, and Z out of the tag To better match
+     * wpilib, we want to apply another rotation so that we get Z up, X out of the tag, and Y to the
+     * right. We apply the following change of basis: X -> Y Y -> Z Z -> X
+     */
+    private static final Rotation3d WPILIB_BASE_ROTATION =
+            new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 1, 0, 0, 0, 1, 1, 0, 0));
 
-        var pose =
+    public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
+        // TODO: Refactor into new pipe?
+        // CameraToTarget _should_ be in opencv-land EDN
+        var nwu =
                 CoordinateSystem.convert(
                         new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
-
-        return pose;
+        return new Pose3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
     }
 
-    public static Pose3d convertApriltagtoPhotonPose(Transform3d cameraToTarget3d) {
-        // CameraToTarget _should_ be in opencv-land EDN
-        var pose =
-                CoordinateSystem.convert(
-                        new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
+    /*
+     * The AprilTag pose rotation outputs are X left, Y down, Z away from the tag with the tag facing
+     * the camera upright and the camera facing the target parallel to the floor. But our OpenCV
+     * solvePNP code would have X left, Y up, Z towards the camera with the target facing the camera
+     * and both parallel to the floor. So we apply a base rotation to the rotation component of the
+     * apriltag pose to make it consistent with the EDN system that OpenCV uses, internally a 180
+     * rotation about the X axis
+     */
+    private static final Rotation3d APRILTAG_BASE_ROTATION =
+            new Rotation3d(VecBuilder.fill(1, 0, 0), Units.degreesToRadians(180));
 
-        // Apply an extra rotation so that at zero pose, X ls left, Y is up, and Z is towards the camera
-        // to a camera facing along the +X axis of the field parallel with the ground plane
-        // So we need a 180 flip about X axis
-        var newRotation = pose.getRotation().rotateBy(new Rotation3d(0, Math.PI, 0));
+    /**
+     * Apply a 180 degree rotation about X to the rotation component of a given Apriltag pose. This
+     * aligns it with the OpenCV poses we use in other places.
+     */
+    public static Transform3d convertApriltagtoOpenCV(Transform3d pose) {
+        var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation());
+        return new Transform3d(pose.getTranslation(), ocvRotation);
+    }
 
-        return new Pose3d(pose.getTranslation(), newRotation);
+    public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) {
+        var angle = rotation.getAngle();
+        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));
     }
 }

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.
+*/

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;
 

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;
 

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;
 

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;
 

photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java

@@ -1,29 +1,40 @@
 /*
- * 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;
 
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import java.util.Arrays;
 
+import org.photonvision.common.util.math.MathUtils;
+
 public class DetectionResult {
     public int getId() {
         return id;
@@ -81,11 +92,11 @@ public class DetectionResult {
         return error2;
     }
 
-    public Pose3d getPoseResult1() {
+    public Transform3d getPoseResult1() {
         return poseResult1;
     }
 
-    public Pose3d getPoseResult2() {
+    public Transform3d getPoseResult2() {
         return poseResult2;
     }
 
@@ -96,9 +107,9 @@ public class DetectionResult {
     double centerX, centerY;
     double[] corners;
 
-    Pose3d poseResult1;
+    Transform3d poseResult1;
     double error1;
-    Pose3d poseResult2;
+    Transform3d poseResult2;
     double error2;
 
     public DetectionResult(
@@ -125,14 +136,29 @@ public class DetectionResult {
 
         this.error1 = err1;
         this.poseResult1 =
-                new Pose3d(
+                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 Pose3d(
+                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))));
+    }
+
+    /**
+     * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
+     * ambiguous.
+     */
+    public double getPoseAmbiguity() {
+        var min = Math.min(error1, error2);
+        var max = Math.max(error1, error2);
+
+        if (max > 0) {
+            return min / max;
+        } else {
+            return -1;
+        }
     }
 
     @Override

photon-core/src/main/java/org/photonvision/vision/camera/CameraQuirk.java

@@ -27,5 +27,7 @@ public enum CameraQuirk {
     /** Separate red/blue gain controls available */
     AWBGain,
     /** Will not work with photonvision - Logitec C270 at least */
-    CompletelyBroken
+    CompletelyBroken,
+    /** Has adjustable focus and autofocus switch */
+    AdjustableFocus,
 }

photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java

@@ -91,15 +91,14 @@ public class FileVisionSource extends VisionSource {
         @Override
         public void setExposure(double exposure) {}
 
+        public void setAutoExposure(boolean cameraAutoExposure) {}
+
         @Override
         public void setBrightness(int brightness) {}
 
         @Override
         public void setGain(int gain) {}
 
-        @Override
-        public void setLowExposureOptimization(boolean mode) {}
-
         @Override
         public VideoMode getCurrentVideoMode() {
             return videoMode;

photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java

@@ -29,9 +29,21 @@ public class QuirkyCamera {
                             0x5A3,
                             CameraQuirk.CompletelyBroken), // Chris's older generic "Logitec HD Webcam"
                     new QuirkyCamera(0x825, 0x46D, CameraQuirk.CompletelyBroken), // Logitec C270
+                    new QuirkyCamera(
+                            0x0bda,
+                            0x5510,
+                            CameraQuirk.CompletelyBroken), // A laptop internal camera someone found broken
+                    new QuirkyCamera(
+                            -1, -1, "Snap Camera", CameraQuirk.CompletelyBroken), // SnapCamera on Windows
+                    new QuirkyCamera(
+                            -1,
+                            -1,
+                            "FaceTime HD Camera",
+                            CameraQuirk.CompletelyBroken), // Mac Facetime Camera shared into Windows in Bootcamp
                     new QuirkyCamera(0x2000, 0x1415, CameraQuirk.Gain, CameraQuirk.FPSCap100), // PS3Eye
                     new QuirkyCamera(
-                            -1, -1, "mmal service 16.1", CameraQuirk.PiCam) // PiCam (via V4L2, not zerocopy)
+                            -1, -1, "mmal service 16.1", CameraQuirk.PiCam), // PiCam (via V4L2, not zerocopy)
+                    new QuirkyCamera(0x85B, 0x46D, CameraQuirk.AdjustableFocus) // Logitech C925-e
                     );
 
     public static final QuirkyCamera DefaultCamera = new QuirkyCamera(0, 0, "");

photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java

@@ -18,10 +18,7 @@
 package org.photonvision.vision.camera;
 
 import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.cscore.CvSink;
-import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.cscore.VideoException;
-import edu.wpi.first.cscore.VideoMode;
+import edu.wpi.first.cscore.*;
 import java.util.*;
 import java.util.stream.Collectors;
 import org.photonvision.common.configuration.CameraConfiguration;
@@ -64,60 +61,26 @@ public class USBCameraSource extends VisionSource {
             usbFrameProvider = null;
         } else {
             // Normal init
-            setLowExposureOptimizationImpl(false);
+            // auto exposure/brightness/gain will be set by the visionmodule later
+            disableAutoFocus();
+
             usbCameraSettables = new USBCameraSettables(config);
-            usbFrameProvider = new USBFrameProvider(cvSink, usbCameraSettables);
+            if (usbCameraSettables.getAllVideoModes().isEmpty()) {
+                logger.info("Camera " + camera.getPath() + " has no video modes supported by PhotonVision");
+                usbFrameProvider = null;
+            } else {
+                usbFrameProvider = new USBFrameProvider(cvSink, usbCameraSettables);
+            }
         }
     }
 
-    void setLowExposureOptimizationImpl(boolean lowExposureMode) {
-        if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
-            // Case, we know this is a picam. Go through v4l2-ctl interface directly
-
-            // Common settings
-            camera
-                    .getProperty("image_stabilization")
-                    .set(0); // No image stabilization, as this will throw off odometry
-            camera.getProperty("power_line_frequency").set(2); // Assume 60Hz USA
-            camera.getProperty("scene_mode").set(0); // no presets
-            camera.getProperty("exposure_metering_mode").set(0);
-            camera.getProperty("exposure_dynamic_framerate").set(0);
-
-            if (lowExposureMode) {
-                // Pick a bunch of reasonable setting defaults for vision processing retroreflective
-                camera.getProperty("auto_exposure_bias").set(0);
-                camera.getProperty("iso_sensitivity_auto").set(0); // Disable auto ISO adjustement
-                camera.getProperty("iso_sensitivity").set(0); // Manual ISO adjustement
-                camera.getProperty("white_balance_auto_preset").set(2); // Auto white-balance disabled
-                camera.getProperty("auto_exposure").set(1); // auto exposure disabled
-            } else {
-                // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise
-                // nice-for-humans
-                camera.getProperty("auto_exposure_bias").set(12);
-                camera.getProperty("iso_sensitivity_auto").set(1);
-                camera.getProperty("iso_sensitivity").set(1); // Manual ISO adjustement by default
-                camera.getProperty("white_balance_auto_preset").set(1); // Auto white-balance enabled
-                camera.getProperty("auto_exposure").set(0); // auto exposure enabled
-            }
-
-        } else {
-            // Case - this is some other USB cam. Default to wpilib's implementation
-
-            var canSetWhiteBalance = !cameraQuirks.hasQuirk(CameraQuirk.Gain);
-
-            if (lowExposureMode) {
-                // Pick a bunch of reasonable setting defaults for vision processing retroreflective
-                if (canSetWhiteBalance) {
-                    camera.setWhiteBalanceManual(4000); // Auto white-balance disabled, 4000K preset
-                }
-                this.getSettables().setExposure(50); // auto exposure disabled, put a sane default
-            } else {
-                // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise
-                // nice-for-humans
-                if (canSetWhiteBalance) {
-                    camera.setWhiteBalanceAuto(); // Auto white-balance enabled
-                }
-                camera.setExposureAuto(); // auto exposure enabled
+    void disableAutoFocus() {
+        if (cameraQuirks.hasQuirk(CameraQuirk.AdjustableFocus)) {
+            try {
+                camera.getProperty("focus_auto").set(0);
+                camera.getProperty("focus_absolute").set(0); // Focus into infinity
+            } catch (VideoException e) {
+                logger.error("Unable to disable autofocus!", e);
             }
         }
     }
@@ -139,6 +102,59 @@ public class USBCameraSource extends VisionSource {
             setVideoMode(videoModes.get(0));
         }
 
+        public void setAutoExposure(boolean cameraAutoExposure) {
+            logger.debug("Setting auto exposure to " + cameraAutoExposure);
+
+            if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
+                // Case, we know this is a picam. Go through v4l2-ctl interface directly
+
+                // Common settings
+                camera
+                        .getProperty("image_stabilization")
+                        .set(0); // No image stabilization, as this will throw off odometry
+                camera.getProperty("power_line_frequency").set(2); // Assume 60Hz USA
+                camera.getProperty("scene_mode").set(0); // no presets
+                camera.getProperty("exposure_metering_mode").set(0);
+                camera.getProperty("exposure_dynamic_framerate").set(0);
+
+                if (!cameraAutoExposure) {
+                    // Pick a bunch of reasonable setting defaults for vision processing retroreflective
+                    camera.getProperty("auto_exposure_bias").set(0);
+                    camera.getProperty("iso_sensitivity_auto").set(0); // Disable auto ISO adjustement
+                    camera.getProperty("iso_sensitivity").set(0); // Manual ISO adjustement
+                    camera.getProperty("white_balance_auto_preset").set(2); // Auto white-balance disabled
+                    camera.getProperty("auto_exposure").set(1); // auto exposure disabled
+                } else {
+                    // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise
+                    // nice-for-humans
+                    camera.getProperty("auto_exposure_bias").set(12);
+                    camera.getProperty("iso_sensitivity_auto").set(1);
+                    camera.getProperty("iso_sensitivity").set(1); // Manual ISO adjustement by default
+                    camera.getProperty("white_balance_auto_preset").set(1); // Auto white-balance enabled
+                    camera.getProperty("auto_exposure").set(0); // auto exposure enabled
+                }
+
+            } else {
+                // Case - this is some other USB cam. Default to wpilib's implementation
+
+                var canSetWhiteBalance = !cameraQuirks.hasQuirk(CameraQuirk.Gain);
+
+                if (!cameraAutoExposure) {
+                    // Pick a bunch of reasonable setting defaults for vision processing retroreflective
+                    if (canSetWhiteBalance) {
+                        camera.setWhiteBalanceManual(4000); // Auto white-balance disabled, 4000K preset
+                    }
+                } else {
+                    // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise
+                    // nice-for-humans
+                    if (canSetWhiteBalance) {
+                        camera.setWhiteBalanceAuto(); // Auto white-balance enabled
+                    }
+                    camera.setExposureAuto(); // auto exposure enabled
+                }
+            }
+        }
+
         private int timeToPiCamRawExposure(double time_us) {
             int retVal =
                     (int)
@@ -157,11 +173,6 @@ public class USBCameraSource extends VisionSource {
                     + (pct_in / 100.0) * ((1e6 / (double) camera.getVideoMode().fps) - PADDING_HIGH_US);
         }
 
-        @Override
-        public void setLowExposureOptimization(boolean mode) {
-            setLowExposureOptimizationImpl(mode);
-        }
-
         @Override
         public void setExposure(double exposure) {
             if (exposure >= 0.0) {

photon-core/src/main/java/org/photonvision/vision/camera/ZeroCopyPicamSource.java

@@ -45,13 +45,6 @@ public class ZeroCopyPicamSource extends VisionSource {
 
         settables = new PicamSettables(configuration);
         frameProvider = new AcceleratedPicamFrameProvider(settables);
-
-        setLowExposureOptimizationImpl(false);
-    }
-
-    static void setLowExposureOptimizationImpl(boolean mode) {
-        // TODO - ZeroCopy does not... yet? ... have the configuration params necessary to make this
-        // work well.
     }
 
     @Override
@@ -94,6 +87,7 @@ public class ZeroCopyPicamSource extends VisionSource {
         private FPSRatedVideoMode currentVideoMode;
         private double lastExposure = 50;
         private int lastBrightness = 50;
+        private boolean lastExposureMode;
         private int lastGain = 50;
         private Pair<Integer, Integer> lastAwbGains = new Pair(18, 18);
 
@@ -150,9 +144,15 @@ public class ZeroCopyPicamSource extends VisionSource {
             return getCurrentVideoMode().fovMultiplier * getConfiguration().FOV;
         }
 
+        @Override
+        public void setAutoExposure(boolean cameraAutoExposure) {
+            lastExposureMode = cameraAutoExposure;
+            // TODO (Matt) -- call PicamJNI's auto exposure function, when that exists
+        }
+
         @Override
         public void setExposure(double exposure) {
-            // Todo - for now, handle auto exposure by using 100% exposure
+            // Todo (Chris) - for now, handle auto exposure by using 100% exposure
             if (exposure < 0.0) {
                 exposure = 100.0;
             }
@@ -162,11 +162,6 @@ public class ZeroCopyPicamSource extends VisionSource {
             if (failure) logger.warn("Couldn't set Pi Camera exposure");
         }
 
-        @Override
-        public void setLowExposureOptimization(boolean mode) {
-            setLowExposureOptimizationImpl(mode);
-        }
-
         @Override
         public void setBrightness(int brightness) {
             lastBrightness = brightness;
@@ -218,6 +213,7 @@ public class ZeroCopyPicamSource extends VisionSource {
             // We don't store last settings on the native side, and when you change video mode these get
             // reset on MMAL's end
             setExposure(lastExposure);
+            setAutoExposure(lastExposureMode);
             setBrightness(lastBrightness);
             setGain(lastGain);
             setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());

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

@@ -25,7 +25,9 @@ import org.opencv.calib3d.Calib3d;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfPoint;
 import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
 import org.opencv.core.Point;
+import org.opencv.core.Point3;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
@@ -43,6 +45,11 @@ public class Draw3dTargetsPipe
     @Override
     protected Void process(Pair<Mat, List<TrackedTarget>> in) {
         if (!params.shouldDraw) return null;
+        if (params.cameraCalibrationCoefficients == null
+                || params.cameraCalibrationCoefficients.getCameraIntrinsicsMat() == null
+                || params.cameraCalibrationCoefficients.getCameraExtrinsicsMat() == null) {
+            return null;
+        }
 
         for (var target : in.getRight()) {
             // draw convex hull
@@ -111,6 +118,54 @@ public class Draw3dTargetsPipe
                             ColorHelper.colorToScalar(Color.green),
                             3);
                 }
+
+                // Draw X, Y and Z axis
+                MatOfPoint3f pointMat = new MatOfPoint3f();
+                // Those points are in opencv-land, but we are in NWU
+                // NWU | EDN
+                // X: Z
+                // Y: -X
+                // Z: -Y
+                final double AXIS_LEN = 0.2;
+                var list =
+                        List.of(
+                                new Point3(0, 0, 0),
+                                new Point3(0, 0, AXIS_LEN),
+                                new Point3(AXIS_LEN, 0, 0),
+                                new Point3(0, AXIS_LEN, 0));
+                pointMat.fromList(list);
+
+                Calib3d.projectPoints(
+                        pointMat,
+                        target.getCameraRelativeRvec(),
+                        target.getCameraRelativeTvec(),
+                        params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(),
+                        params.cameraCalibrationCoefficients.getCameraExtrinsicsMat(),
+                        tempMat,
+                        jac);
+                var axisPoints = tempMat.toList();
+                dividePointList(axisPoints);
+
+                // Red = x, green y, blue z
+                Imgproc.line(
+                        in.getLeft(),
+                        axisPoints.get(0),
+                        axisPoints.get(2),
+                        ColorHelper.colorToScalar(Color.GREEN),
+                        3);
+                Imgproc.line(
+                        in.getLeft(),
+                        axisPoints.get(0),
+                        axisPoints.get(3),
+                        ColorHelper.colorToScalar(Color.BLUE),
+                        3);
+                Imgproc.line(
+                        in.getLeft(),
+                        axisPoints.get(0),
+                        axisPoints.get(1),
+                        ColorHelper.colorToScalar(Color.RED),
+                        3);
+
                 for (int i = 0; i < bottomPoints.size(); i++) {
                     Imgproc.line(
                             in.getLeft(),
@@ -127,8 +182,10 @@ public class Draw3dTargetsPipe
                             ColorHelper.colorToScalar(Color.orange),
                             3);
                 }
+
                 tempMat.release();
                 jac.release();
+                pointMat.release();
             }
 
             // draw corners

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

@@ -100,8 +100,9 @@ public class SolvePNPPipe
                         Core.norm(rVec));
 
         Pose3d targetPose = MathUtils.convertOpenCVtoPhotonPose(new Transform3d(translation, rotation));
-        target.setCameraToTarget3d(
+        target.setBestCameraToTarget3d(
                 new Transform3d(targetPose.getTranslation(), targetPose.getRotation()));
+        target.setAltCameraToTarget3d(new Transform3d());
     }
 
     Mat rotationMatrix = new Mat();

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;
                 }
         }
@@ -117,9 +127,13 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
             sumPipeNanosElapsed += rotateImageResult.nanosElapsed;
         }
 
+        var inputFrame = new Frame(new CVMat(rawInputMat), frameStaticProperties);
+
         grayscalePipeResult = grayscalePipe.run(rawInputMat);
         sumPipeNanosElapsed += grayscalePipeResult.nanosElapsed;
 
+        var outputFrame = new Frame(new CVMat(grayscalePipeResult.output), frameStaticProperties);
+
         List<TrackedTarget> targetList;
         CVPipeResult<List<DetectionResult>> tagDetectionPipeResult;
 
@@ -127,11 +141,14 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
         aprilTagDetectionPipe.setNativePoseEstimationEnabled(settings.solvePNPEnabled);
 
         tagDetectionPipeResult = aprilTagDetectionPipe.run(grayscalePipeResult.output);
-        grayscalePipeResult.output.release();
         sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
 
         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 =
@@ -140,9 +157,13 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
                             new TargetCalculationParameters(
                                     false, null, null, null, null, frameStaticProperties));
 
-            var correctedPose = MathUtils.convertApriltagtoPhotonPose(target.getCameraToTarget3d());
-            target.setCameraToTarget3d(
-                    new Transform3d(correctedPose.getTranslation(), correctedPose.getRotation()));
+            var correctedBestPose = MathUtils.convertOpenCVtoPhotonPose(target.getBestCameraToTarget3d());
+            var correctedAltPose = MathUtils.convertOpenCVtoPhotonPose(target.getAltCameraToTarget3d());
+
+            target.setBestCameraToTarget3d(
+                    new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
+            target.setAltCameraToTarget3d(
+                    new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
 
             targetList.add(target);
         }
@@ -150,11 +171,6 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
-        var inputFrame = new Frame(new CVMat(rawInputMat), frameStaticProperties);
-        // empty output frame
-        var outputFrame =
-                Frame.emptyFrame(frameStaticProperties.imageWidth, frameStaticProperties.imageHeight);
-
         return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, outputFrame, inputFrame);
     }
 }

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() {
@@ -40,6 +44,7 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
         outputShowMultipleTargets = true;
         targetModel = TargetModel.k200mmAprilTag;
         cameraExposure = -1;
+        cameraAutoExposure = true;
         ledMode = false;
     }
 

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

@@ -40,7 +40,8 @@ public class CVPipelineSettings implements Cloneable {
     public ImageFlipMode inputImageFlipMode = ImageFlipMode.NONE;
     public ImageRotationMode inputImageRotationMode = ImageRotationMode.DEG_0;
     public String pipelineNickname = "New Pipeline";
-    // Only used if the pipeline type does not enable auto-exposure
+    public boolean cameraAutoExposure = false;
+    // manual exposure only used if cameraAutoExposure if false
     public double cameraExposure = 100;
     public int cameraBrightness = 50;
     // Currently only used by a few cameras (notably the zero-copy Pi Camera driver) with the Gain

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

@@ -120,42 +120,58 @@ public class OutputStreamPipeline {
             pipeProfileNanos[2] = 0;
         }
 
-        // Draw 2D Crosshair on input and output
-        var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
+        // Draw 2D Crosshair on output
+        var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
         sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
 
         if (!(settings instanceof AprilTagPipelineSettings)) {
+            // If we're processing anything other than Apriltags...
+
             var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
             sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
 
-            // Draw 3D Targets on input and output if necessary
             if (settings.solvePNPEnabled
                     || (settings.solvePNPEnabled
                             && settings instanceof ColoredShapePipelineSettings
                             && ((ColoredShapePipelineSettings) settings).contourShape == ContourShape.Circle)) {
-                var drawOnInputResult = draw3dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+                // Draw 3D Targets on input and output if possible
+                pipeProfileNanos[5] = 0;
+                pipeProfileNanos[6] = 0;
+                pipeProfileNanos[7] = 0;
 
                 var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
                 sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
             } else {
-                pipeProfileNanos[7] = 0;
-                pipeProfileNanos[8] = 0;
-
-                var draw2dTargetsOnInput = draw2dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+                // Only draw 2d targets
+                pipeProfileNanos[5] = 0;
 
                 var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
                 sumPipeNanosElapsed += pipeProfileNanos[6] = draw2dTargetsOnOutput.nanosElapsed;
+
+                pipeProfileNanos[7] = 0;
+                pipeProfileNanos[8] = 0;
             }
 
         } else {
+            // If we are doing apriltags...
             if (settings.solvePNPEnabled) {
-                var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(inMat, targetsToDraw));
+                // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
+                pipeProfileNanos[5] = 0;
+                pipeProfileNanos[6] = 0;
+
+                var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
                 sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+
+                pipeProfileNanos[8] = 0;
+
             } else {
-                var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(inMat, targetsToDraw));
+                // Draw 2d apriltag markers
+                var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
                 sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+
+                pipeProfileNanos[6] = 0;
+                pipeProfileNanos[7] = 0;
+                pipeProfileNanos[8] = 0;
             }
         }
 

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

@@ -181,16 +181,23 @@ public class PipelineManager {
             var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
             switch (desiredPipelineSettings.pipelineType) {
                 case Reflective:
+                    logger.debug("Creating Reflective pipeline");
                     currentUserPipeline =
                             new ReflectivePipeline((ReflectivePipelineSettings) desiredPipelineSettings);
                     break;
                 case ColoredShape:
+                    logger.debug("Creating ColoredShape pipeline");
                     currentUserPipeline =
                             new ColoredShapePipeline((ColoredShapePipelineSettings) desiredPipelineSettings);
                     break;
                 case AprilTag:
+                    logger.debug("Creating AprilTag pipeline");
                     currentUserPipeline =
                             new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings);
+                    break;
+                default:
+                    // Can be calib3d or drivermode, both of which are special cases
+                    break;
             }
         }
     }

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

@@ -17,7 +17,7 @@
 
 package org.photonvision.vision.processes;
 
-import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.cscore.VideoException;
 import edu.wpi.first.math.util.Units;
 import io.javalin.websocket.WsContext;
 import java.util.*;
@@ -41,15 +41,15 @@ import org.photonvision.vision.camera.USBCameraSource;
 import org.photonvision.vision.camera.ZeroCopyPicamSource;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.consumer.FileSaveFrameConsumer;
-import org.photonvision.vision.frame.consumer.MJPGFrameConsumer;
 import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
 import org.photonvision.vision.pipeline.OutputStreamPipeline;
-import org.photonvision.vision.pipeline.PipelineType;
 import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
 import org.photonvision.vision.pipeline.UICalibrationData;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TargetModel;
 import org.photonvision.vision.target.TrackedTarget;
+import org.photonvision.vision.videoStream.SocketVideoStream;
+import org.photonvision.vision.videoStream.SocketVideoStreamManager;
 
 /**
  * This is the God Class
@@ -58,32 +58,31 @@ import org.photonvision.vision.target.TrackedTarget;
  * provide info on settings changes. VisionModuleManager holds a list of all current vision modules.
  */
 public class VisionModule {
-    private static final int streamFPSCap = 30;
-
     private final Logger logger;
     protected final PipelineManager pipelineManager;
     protected final VisionSource visionSource;
     private final VisionRunner visionRunner;
     private final StreamRunnable streamRunnable;
     private final LinkedList<CVPipelineResultConsumer> resultConsumers = new LinkedList<>();
-    private final LinkedList<CVPipelineResultConsumer> fpsLimitedResultConsumers = new LinkedList<>();
     // Raw result consumers run before any drawing has been done by the OutputStreamPipeline
-    private final LinkedList<TriConsumer<Frame, Frame, List<TrackedTarget>>> rawResultConsumers =
+    private final LinkedList<TriConsumer<Frame, Frame, List<TrackedTarget>>> streamResultConsumers =
             new LinkedList<>();
     private final NTDataPublisher ntConsumer;
     private final UIDataPublisher uiDataConsumer;
     protected final int moduleIndex;
     protected final QuirkyCamera cameraQuirks;
 
-    private long lastFrameConsumeMillis;
     protected TrackedTarget lastPipelineResultBestTarget;
 
-    MJPGFrameConsumer dashboardInputStreamer;
-    MJPGFrameConsumer dashboardOutputStreamer;
+    private int inputStreamPort = -1;
+    private int outputStreamPort = -1;
 
     FileSaveFrameConsumer inputFrameSaver;
     FileSaveFrameConsumer outputFrameSaver;
 
+    SocketVideoStream inputVideoStreamer;
+    SocketVideoStream outputVideoStreamer;
+
     public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) {
         logger =
                 new Logger(
@@ -131,7 +130,7 @@ public class VisionModule {
 
         createStreams();
 
-        recreateFpsLimitedResultConsumers();
+        recreateStreamResultConsumers();
 
         ntConsumer =
                 new NTDataPublisher(
@@ -168,49 +167,33 @@ public class VisionModule {
     }
 
     private void destroyStreams() {
-        dashboardInputStreamer.close();
-        dashboardOutputStreamer.close();
+        SocketVideoStreamManager.getInstance().removeStream(inputVideoStreamer);
+        SocketVideoStreamManager.getInstance().removeStream(outputVideoStreamer);
     }
 
     private void createStreams() {
         var camStreamIdx = visionSource.getSettables().getConfiguration().streamIndex;
         // If idx = 0, we want (1181, 1182)
-        var inputStreamPort = 1181 + (camStreamIdx * 2);
-        var outputStreamPort = 1181 + (camStreamIdx * 2) + 1;
-
-        dashboardOutputStreamer =
-                new MJPGFrameConsumer(
-                        visionSource.getSettables().getConfiguration().nickname + "-output", outputStreamPort);
-        dashboardInputStreamer =
-                new MJPGFrameConsumer(
-                        visionSource.getSettables().getConfiguration().uniqueName + "-input", inputStreamPort);
+        this.inputStreamPort = 1181 + (camStreamIdx * 2);
+        this.outputStreamPort = 1181 + (camStreamIdx * 2) + 1;
 
         inputFrameSaver =
                 new FileSaveFrameConsumer(visionSource.getSettables().getConfiguration().nickname, "input");
         outputFrameSaver =
                 new FileSaveFrameConsumer(
                         visionSource.getSettables().getConfiguration().nickname, "output");
+
+        inputVideoStreamer = new SocketVideoStream(this.inputStreamPort);
+        outputVideoStreamer = new SocketVideoStream(this.outputStreamPort);
+        SocketVideoStreamManager.getInstance().addStream(inputVideoStreamer);
+        SocketVideoStreamManager.getInstance().addStream(outputVideoStreamer);
     }
 
-    private void recreateFpsLimitedResultConsumers() {
-        // Important! These must come before the stream result consumers because the stream result
-        // consumers release the frame
-        rawResultConsumers.add((in, out, tgts) -> inputFrameSaver.accept(in));
-        fpsLimitedResultConsumers.add(result -> outputFrameSaver.accept(result.outputFrame));
-
-        fpsLimitedResultConsumers.add(
-                result -> {
-                    if (this.pipelineManager.getCurrentPipelineSettings().inputShouldShow)
-                        dashboardInputStreamer.accept(result.inputFrame);
-                    else dashboardInputStreamer.disabledTick();
-                });
-        fpsLimitedResultConsumers.add(
-                result -> {
-                    if (this.pipelineManager.getCurrentPipelineSettings().outputShouldShow)
-                        dashboardOutputStreamer.accept(result.outputFrame);
-                    else dashboardInputStreamer.disabledTick();
-                    ;
-                });
+    private void recreateStreamResultConsumers() {
+        streamResultConsumers.add((in, out, tgts) -> inputFrameSaver.accept(in));
+        streamResultConsumers.add((in, out, tgts) -> outputFrameSaver.accept(out));
+        streamResultConsumers.add((in, out, tgts) -> inputVideoStreamer.accept(in));
+        streamResultConsumers.add((in, out, tgts) -> outputVideoStreamer.accept(out));
     }
 
     private class StreamRunnable extends Thread {
@@ -272,12 +255,11 @@ public class VisionModule {
                     this.shouldRun = false;
                 }
                 if (shouldRun) {
-                    consumeRawResults(inputFrame, outputFrame, targets);
                     try {
                         CVPipelineResult osr =
                                 outputStreamPipeline.process(inputFrame, outputFrame, settings, targets);
+                        consumeResults(inputFrame, osr.outputFrame, targets);
 
-                        consumeFpsLimitedResult(osr);
                     } catch (Exception e) {
                         // Never die
                         logger.error("Exception while running stream runnable!", e);
@@ -305,7 +287,7 @@ public class VisionModule {
         streamRunnable.start();
     }
 
-    public void setFovAndPitch(double fov, Rotation2d pitch) {
+    public void setFov(double fov) {
         var settables = visionSource.getSettables();
         logger.trace(() -> "Setting " + settables.getConfiguration().nickname + ") FOV (" + fov + ")");
 
@@ -361,7 +343,7 @@ public class VisionModule {
             settings.cameraBlueGain = -1;
         }
 
-        settings.cameraExposure = -1;
+        settings.cameraAutoExposure = true;
 
         setPipeline(PipelineManager.CAL_3D_INDEX);
     }
@@ -400,23 +382,19 @@ public class VisionModule {
         visionSource.getSettables().setBrightness(pipelineSettings.cameraBrightness);
         visionSource.getSettables().setGain(pipelineSettings.cameraGain);
 
-        // set to true to change camera gain/exposure settings for low exposure
-        // false will keep the camera running in a more "nice-for-humans" mode
-        // Each camera type is allowed to decide what settings that exactly means
-        boolean lowExposureOptimization =
-                (pipelineSettings.pipelineType == PipelineType.ColoredShape
-                        || pipelineSettings.pipelineType == PipelineType.Reflective);
-        visionSource.getSettables().setLowExposureOptimization(lowExposureOptimization);
-
-        if (lowExposureOptimization) {
-            if (pipelineSettings.cameraExposure == -1)
+        // If manual exposure, force exposure slider to be valid
+        if (!pipelineSettings.cameraAutoExposure) {
+            if (pipelineSettings.cameraExposure < 0)
                 pipelineSettings.cameraExposure = 10; // reasonable default
-        } else {
-            // in human-friendly mode, exposure is automatic
-            pipelineSettings.cameraExposure = -1;
         }
-        visionSource.getSettables().setExposure(pipelineSettings.cameraExposure);
 
+        visionSource.getSettables().setExposure(pipelineSettings.cameraExposure);
+        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;
@@ -483,14 +461,14 @@ public class VisionModule {
         outputFrameSaver.updateCameraNickname(newName);
 
         // Rename streams
-        fpsLimitedResultConsumers.clear();
+        streamResultConsumers.clear();
 
         // Teardown and recreate streams
         destroyStreams();
         createStreams();
 
         // Rebuild streamers
-        recreateFpsLimitedResultConsumers();
+        recreateStreamResultConsumers();
 
         // Push new data to the UI
         saveAndBroadcastAll();
@@ -525,8 +503,8 @@ public class VisionModule {
             temp.put(k, internalMap);
         }
         ret.videoFormatList = temp;
-        ret.outputStreamPort = dashboardOutputStreamer.getCurrentStreamPort();
-        ret.inputStreamPort = dashboardInputStreamer.getCurrentStreamPort();
+        ret.outputStreamPort = this.outputStreamPort;
+        ret.inputStreamPort = this.inputStreamPort;
 
         var calList = new ArrayList<HashMap<String, Object>>();
         for (var c : visionSource.getSettables().getConfiguration().calibrations) {
@@ -576,7 +554,7 @@ public class VisionModule {
                     result.targets);
             // The streamRunnable manages releasing in this case
         } else {
-            consumeFpsLimitedResult(result);
+            consumeResults(result.inputFrame, result.outputFrame, result.targets);
 
             result.release();
             // In this case we don't bother with a separate streaming thread and we release
@@ -589,19 +567,9 @@ public class VisionModule {
         }
     }
 
-    private void consumeFpsLimitedResult(CVPipelineResult result) {
-        long dt = System.currentTimeMillis() - lastFrameConsumeMillis;
-        if (dt > 1000 / streamFPSCap) {
-            for (var c : fpsLimitedResultConsumers) {
-                c.accept(result);
-            }
-            lastFrameConsumeMillis = System.currentTimeMillis();
-        }
-    }
-
-    /** Consume results prior to drawing on them. */
-    private void consumeRawResults(Frame inputFrame, Frame outputFrame, List<TrackedTarget> targets) {
-        for (var c : rawResultConsumers) {
+    /** Consume stream/target results, no rate limiting applied */
+    private void consumeResults(Frame inputFrame, Frame outputFrame, List<TrackedTarget> targets) {
+        for (var c : streamResultConsumers) {
             c.accept(inputFrame, outputFrame, targets);
         }
     }

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

@@ -318,7 +318,8 @@ public class VisionSourceManager {
 
             var newCam = new USBCameraSource(configuration);
 
-            if (!newCam.cameraQuirks.hasQuirk(CameraQuirk.CompletelyBroken)) {
+            if (!newCam.cameraQuirks.hasQuirk(CameraQuirk.CompletelyBroken)
+                    && !newCam.getSettables().videoModes.isEmpty()) {
                 cameraSources.add(newCam);
             }
         }

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

@@ -44,6 +44,8 @@ public abstract class VisionSourceSettables {
 
     public abstract void setExposure(double exposure);
 
+    public abstract void setAutoExposure(boolean cameraAutoExposure);
+
     public abstract void setBrightness(int brightness);
 
     public abstract void setGain(int gain);
@@ -64,8 +66,6 @@ public abstract class VisionSourceSettables {
         calculateFrameStaticProps();
     }
 
-    public abstract void setLowExposureOptimization(boolean mode);
-
     protected abstract void setVideoModeInternal(VideoMode videoMode);
 
     @SuppressWarnings("unused")

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

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

@@ -16,7 +16,6 @@
  */
 package org.photonvision.vision.target;
 
-import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import java.util.HashMap;
 import java.util.List;
@@ -26,6 +25,7 @@ import org.opencv.core.MatOfPoint;
 import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
 import org.opencv.core.RotatedRect;
+import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.apriltag.DetectionResult;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.*;
@@ -46,11 +46,13 @@ public class TrackedTarget implements Releasable {
     private double m_area;
     private double m_skew;
 
-    private Transform3d m_cameraToTarget3d = new Transform3d();
+    private Transform3d m_bestCameraToTarget3d = new Transform3d();
+    private Transform3d m_altCameraToTarget3d = new Transform3d();
 
     private CVShape m_shape;
 
     private int m_fiducialId = -1;
+    private double m_poseAmbiguity = -1;
 
     private Mat m_cameraRelativeTvec, m_cameraRelativeRvec;
 
@@ -72,14 +74,21 @@ public class TrackedTarget implements Releasable {
         m_yaw =
                 TargetCalculations.calculateYaw(
                         result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
-        Pose3d bestPose = new Pose3d();
+        var bestPose = new Transform3d();
+        var altPose = new Transform3d();
         if (result.getError1() <= result.getError2()) {
             bestPose = result.getPoseResult1();
+            altPose = result.getPoseResult2();
         } else {
             bestPose = result.getPoseResult2();
+            altPose = result.getPoseResult1();
         }
 
-        m_cameraToTarget3d = new Transform3d(new Pose3d(), bestPose);
+        bestPose = MathUtils.convertApriltagtoOpenCV(bestPose);
+        altPose = MathUtils.convertApriltagtoOpenCV(altPose);
+
+        m_bestCameraToTarget3d = bestPose;
+        m_altCameraToTarget3d = altPose;
 
         double[] corners = result.getCorners();
         Point[] cornerPoints =
@@ -113,10 +122,10 @@ public class TrackedTarget implements Releasable {
 
         // Opencv expects a 3d vector with norm = angle and direction = axis
         var rvec = new Mat(3, 1, CvType.CV_64FC1);
-        var angle = bestPose.getRotation().getAngle();
-        var axis = bestPose.getRotation().getAxis().times(angle);
-        rvec.put(0, 0, axis.getData());
+        MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
         setCameraRelativeRvec(rvec);
+
+        m_poseAmbiguity = result.getPoseAmbiguity();
     }
 
     public void setFiducialId(int id) {
@@ -127,6 +136,14 @@ public class TrackedTarget implements Releasable {
         return m_fiducialId;
     }
 
+    public void setPoseAmbiguity(double ambiguity) {
+        m_poseAmbiguity = ambiguity;
+    }
+
+    public double getPoseAmbiguity() {
+        return m_poseAmbiguity;
+    }
+
     /**
      * Set the approximate bouding polygon.
      *
@@ -220,12 +237,20 @@ public class TrackedTarget implements Releasable {
         return !m_subContours.isEmpty();
     }
 
-    public Transform3d getCameraToTarget3d() {
-        return m_cameraToTarget3d;
+    public Transform3d getBestCameraToTarget3d() {
+        return m_bestCameraToTarget3d;
     }
 
-    public void setCameraToTarget3d(Transform3d pose) {
-        this.m_cameraToTarget3d = pose;
+    public Transform3d getAltCameraToTarget3d() {
+        return m_altCameraToTarget3d;
+    }
+
+    public void setBestCameraToTarget3d(Transform3d pose) {
+        this.m_bestCameraToTarget3d = pose;
+    }
+
+    public void setAltCameraToTarget3d(Transform3d pose) {
+        this.m_altCameraToTarget3d = pose;
     }
 
     public Mat getCameraRelativeTvec() {
@@ -260,8 +285,9 @@ public class TrackedTarget implements Releasable {
         ret.put("yaw", getYaw());
         ret.put("skew", getSkew());
         ret.put("area", getArea());
-        if (getCameraToTarget3d() != null) {
-            ret.put("pose", transformToMap(getCameraToTarget3d()));
+        ret.put("ambiguity", getPoseAmbiguity());
+        if (getBestCameraToTarget3d() != null) {
+            ret.put("pose", transformToMap(getBestCameraToTarget3d()));
         }
         ret.put("fiducialId", getFiducialId());
         return ret;
@@ -277,7 +303,7 @@ public class TrackedTarget implements Releasable {
         ret.put("qy", transform.getRotation().getQuaternion().getY());
         ret.put("qz", transform.getRotation().getQuaternion().getZ());
 
-        ret.put("angle_z", transform.getRotation().getZ() + Math.PI / 2.0);
+        ret.put("angle_z", transform.getRotation().getZ());
         return ret;
     }
 

photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStream.java

@@ -0,0 +1,116 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.videoStream;
+
+import java.nio.ByteBuffer;
+import java.util.Base64;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Consumer;
+import org.opencv.core.MatOfByte;
+import org.opencv.core.MatOfInt;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.frame.consumer.MJPGFrameConsumer;
+
+public class SocketVideoStream implements Consumer<Frame> {
+    int portID = 0; // Align with cscore's port for unique identification of stream
+    MatOfByte jpegBytes = null;
+
+    // Gets set to true when another class reads out valid jpeg bytes at least once
+    // Set back to false when another frame is freshly converted
+    // Should eliminate synchronization issues of differeing rates of putting frames in
+    // and taking them back out
+    boolean frameWasConsumed = false;
+
+    // Synclock around manipulating the jpeg bytes from multiple threads
+    Lock jpegBytesLock = new ReentrantLock();
+
+    MJPGFrameConsumer oldSchoolServer;
+
+    private int userCount = 0;
+
+    public SocketVideoStream(int portID) {
+        this.portID = portID;
+        oldSchoolServer =
+                new MJPGFrameConsumer("Port_" + Integer.toString(portID) + "_MJPEG_Server", portID);
+    }
+
+    @Override
+    public void accept(Frame frame) {
+        if (userCount > 0) {
+            if (jpegBytesLock
+                    .tryLock()) { // we assume frames are coming in frequently. Just skip this frame if we're
+                // locked doing something else.
+                try {
+                    // Does a single-shot frame recieve and convert to JPEG for efficency
+                    // Will not capture/convert again until convertNextFrame() is called
+                    if (frame != null && !frame.image.getMat().empty() && jpegBytes == null) {
+                        frameWasConsumed = false;
+                        jpegBytes = new MatOfByte();
+                        Imgcodecs.imencode(
+                                ".jpg",
+                                frame.image.getMat(),
+                                jpegBytes,
+                                new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 75));
+                    }
+                } finally {
+                    jpegBytesLock.unlock();
+                }
+            }
+        }
+        oldSchoolServer.accept(frame);
+    }
+
+    public String getJPEGBase64EncodedStr() {
+        String sendStr = null;
+        jpegBytesLock.lock();
+        if (jpegBytes != null) {
+            sendStr = Base64.getEncoder().encodeToString(jpegBytes.toArray());
+        }
+        jpegBytesLock.unlock();
+        return sendStr;
+    }
+
+    public ByteBuffer getJPEGByteBuffer() {
+        ByteBuffer sendStr = null;
+        jpegBytesLock.lock();
+        if (jpegBytes != null) {
+            sendStr = ByteBuffer.wrap(jpegBytes.toArray());
+        }
+        jpegBytesLock.unlock();
+        return sendStr;
+    }
+
+    public void convertNextFrame() {
+        jpegBytesLock.lock();
+        if (jpegBytes != null) {
+            jpegBytes.release();
+            jpegBytes = null;
+        }
+        jpegBytesLock.unlock();
+    }
+
+    public void addUser() {
+        userCount++;
+    }
+
+    public void removeUser() {
+        userCount--;
+    }
+}

photon-core/src/main/java/org/photonvision/vision/videoStream/SocketVideoStreamManager.java

@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.videoStream;
+
+import io.javalin.websocket.WsContext;
+import java.nio.ByteBuffer;
+import java.util.Hashtable;
+import java.util.Map;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class SocketVideoStreamManager {
+    private static final int NO_STREAM_PORT = -1;
+
+    private final Logger logger = new Logger(SocketVideoStreamManager.class, LogGroup.Camera);
+
+    private Map<Integer, SocketVideoStream> streams = new Hashtable<Integer, SocketVideoStream>();
+    private Map<WsContext, Integer> userSubscriptions = new Hashtable<WsContext, Integer>();
+
+    private static class ThreadSafeSingleton {
+        private static final SocketVideoStreamManager INSTANCE = new SocketVideoStreamManager();
+    }
+
+    public static SocketVideoStreamManager getInstance() {
+        return ThreadSafeSingleton.INSTANCE;
+    }
+
+    private SocketVideoStreamManager() {}
+
+    // Register a new available camera stream
+    public void addStream(SocketVideoStream newStream) {
+        streams.put(newStream.portID, newStream);
+        logger.debug("Added new stream for port " + Integer.toString(newStream.portID));
+    }
+
+    // Remove a previously-added camera stream, and unsubscribe all users
+    public void removeStream(SocketVideoStream oldStream) {
+        streams.remove(oldStream.portID);
+        logger.debug("Removed stream for port " + Integer.toString(oldStream.portID));
+    }
+
+    // Indicate a user would like to subscribe to a camera stream and get frames from it periodically
+    public void addSubscription(WsContext user, int streamPortID) {
+        var stream = streams.get(streamPortID);
+        if (stream != null) {
+            userSubscriptions.put(user, streamPortID);
+            stream.addUser();
+        } else {
+            logger.error(
+                    "User attempted to subscribe to non-existent port " + Integer.toString(streamPortID));
+        }
+    }
+
+    // Indicate a user would like to stop receiving one camera stream
+    public void removeSubscription(WsContext user) {
+        var port = userSubscriptions.get(user);
+        if (port != null && port != NO_STREAM_PORT) {
+            var stream = streams.get(port);
+            userSubscriptions.put(user, NO_STREAM_PORT);
+            if (stream != null) {
+                stream.removeUser();
+            }
+        } else {
+            logger.error(
+                    "User attempted to unsubscribe, but had not yet previously subscribed successfully.");
+        }
+    }
+
+    // For a given user, return the jpeg bytes (or null) for the most recent frame
+    public ByteBuffer getSendFrame(WsContext user) {
+        var port = userSubscriptions.get(user);
+        if (port != null && port != NO_STREAM_PORT) {
+            var stream = streams.get(port);
+            return stream.getJPEGByteBuffer();
+        } else {
+            return null;
+        }
+    }
+
+    // Causes all streams to "re-trigger" and recieve and convert their next mjpeg frame
+    // Only invoke this after all returned jpeg Strings have been used.
+    public void allStreamConvertNextFrame() {
+        for (SocketVideoStream stream : streams.values()) {
+            stream.convertNextFrame();
+        }
+    }
+}

photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java

@@ -30,6 +30,7 @@ import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.common.util.file.JacksonUtils;
+import org.photonvision.vision.pipeline.AprilTagPipelineSettings;
 import org.photonvision.vision.pipeline.ColoredShapePipelineSettings;
 import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
 import org.photonvision.vision.target.TargetModel;
@@ -40,6 +41,7 @@ public class ConfigTest {
             new CameraConfiguration("TestCamera", "/dev/video420");
     private static ReflectivePipelineSettings REFLECTIVE_PIPELINE_SETTINGS;
     private static ColoredShapePipelineSettings COLORED_SHAPE_PIPELINE_SETTINGS;
+    private static AprilTagPipelineSettings APRIL_TAG_PIPELINE_SETTINGS;
 
     @BeforeAll
     public static void init() {
@@ -51,6 +53,7 @@ public class ConfigTest {
 
         REFLECTIVE_PIPELINE_SETTINGS = new ReflectivePipelineSettings();
         COLORED_SHAPE_PIPELINE_SETTINGS = new ColoredShapePipelineSettings();
+        APRIL_TAG_PIPELINE_SETTINGS = new AprilTagPipelineSettings();
 
         REFLECTIVE_PIPELINE_SETTINGS.pipelineNickname = "2019Tape";
         REFLECTIVE_PIPELINE_SETTINGS.targetModel = TargetModel.k2019DualTarget;
@@ -58,8 +61,12 @@ public class ConfigTest {
         COLORED_SHAPE_PIPELINE_SETTINGS.pipelineNickname = "2019Cargo";
         COLORED_SHAPE_PIPELINE_SETTINGS.pipelineIndex = 1;
 
+        APRIL_TAG_PIPELINE_SETTINGS.pipelineNickname = "apriltag";
+        APRIL_TAG_PIPELINE_SETTINGS.pipelineIndex = 2;
+
         cameraConfig.addPipelineSetting(REFLECTIVE_PIPELINE_SETTINGS);
         cameraConfig.addPipelineSetting(COLORED_SHAPE_PIPELINE_SETTINGS);
+        cameraConfig.addPipelineSetting(APRIL_TAG_PIPELINE_SETTINGS);
     }
 
     @Test
@@ -90,9 +97,12 @@ public class ConfigTest {
                 configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(0);
         var coloredShapePipelineSettings =
                 configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(1);
+        var apriltagPipelineSettings =
+                configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(2);
 
         Assertions.assertEquals(REFLECTIVE_PIPELINE_SETTINGS, reflectivePipelineSettings);
         Assertions.assertEquals(COLORED_SHAPE_PIPELINE_SETTINGS, coloredShapePipelineSettings);
+        Assertions.assertEquals(APRIL_TAG_PIPELINE_SETTINGS, apriltagPipelineSettings);
 
         Assertions.assertTrue(
                 reflectivePipelineSettings instanceof ReflectivePipelineSettings,
@@ -100,6 +110,9 @@ public class ConfigTest {
         Assertions.assertTrue(
                 coloredShapePipelineSettings instanceof ColoredShapePipelineSettings,
                 "Conig loaded pipeline settings for index 1 not of expected type ColoredShapePipelineSettings!");
+        Assertions.assertTrue(
+                apriltagPipelineSettings instanceof AprilTagPipelineSettings,
+                "Conig loaded pipeline settings for index 2 not of expected type AprilTagPipelineSettings!");
     }
 
     @AfterAll

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

@@ -161,7 +161,7 @@ public class CirclePNPTest {
         System.out.println(
                 "Found targets at "
                         + pipelineResult.targets.stream()
-                                .map(TrackedTarget::getCameraToTarget3d)
+                                .map(TrackedTarget::getBestCameraToTarget3d)
                                 .collect(Collectors.toList()));
     }
 }

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

@@ -111,19 +111,19 @@ public class SolvePNPTest {
         printTestResults(pipelineResult);
 
         // these numbers are not *accurate*, but they are known and expected
-        var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
         Assertions.assertEquals(1.1, pose.getTranslation().getX(), 0.05);
         Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.05);
 
-        // We expect the object X axis to be to the right, or negative-Y in world space
+        // We expect the object X to be forward, or -X in world space
         Assertions.assertEquals(
-                -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY(), 0.05);
-        // We expect the object Y axis to be up, or +Z in world space
+                -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.05);
+        // We expect the object Y axis to be right, or negative-Y in world space
         Assertions.assertEquals(
-                1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ(), 0.05);
-        // We expect the object Z axis to towards the camera, or negative-X in world space
+                -1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.05);
+        // We expect the object Z axis to be up, or +Z in world space
         Assertions.assertEquals(
-                -1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX(), 0.05);
+                1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.05);
 
         TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999);
     }
@@ -150,11 +150,20 @@ public class SolvePNPTest {
         CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
         printTestResults(pipelineResult);
 
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        outputPipe.process(
+                pipelineResult.inputFrame,
+                pipelineResult.outputFrame,
+                pipeline.getSettings(),
+                pipelineResult.targets);
+
         // these numbers are not *accurate*, but they are known and expected
-        var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
         Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05);
         Assertions.assertEquals(Units.inchesToMeters(35), pose.getTranslation().getY(), 0.05);
-        Assertions.assertEquals(Units.degreesToRadians(-42), pose.getRotation().getZ(), 1);
+        // Z rotation should be mostly facing us
+        Assertions.assertEquals(Units.degreesToRadians(-140), pose.getRotation().getZ(), 1);
 
         TestUtils.showImage(pipelineResult.inputFrame.image.getMat(), "Pipeline output", 999999);
     }
@@ -202,7 +211,7 @@ public class SolvePNPTest {
         System.out.println(
                 "Found targets at "
                         + pipelineResult.targets.stream()
-                                .map(TrackedTarget::getCameraToTarget3d)
+                                .map(TrackedTarget::getBestCameraToTarget3d)
                                 .collect(Collectors.toList()));
     }
 }

photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java

@@ -78,9 +78,6 @@ public class VisionModuleManagerTest {
         @Override
         public void setGain(int gain) {}
 
-        @Override
-        public void setLowExposureOptimization(boolean mode) {}
-
         @Override
         public VideoMode getCurrentVideoMode() {
             return new VideoMode(0, 320, 240, 254);
@@ -97,6 +94,9 @@ public class VisionModuleManagerTest {
             ret.put(0, getCurrentVideoMode());
             return ret;
         }
+
+        @Override
+        public void setAutoExposure(boolean cameraAutoExposure) {}
     }
 
     private static class TestDataConsumer implements CVPipelineResultConsumer {

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

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

@@ -28,6 +28,7 @@ import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.targeting.PhotonPipelineResult;
@@ -46,6 +47,8 @@ public class PhotonCamera {
     private final String path;
 
     private static boolean VERSION_CHECK_ENABLED = true;
+    private static long VERSION_CHECK_INTERVAL = 1;
+    private double lastVersionCheckTime = 0;
 
     public static void setVersionCheckEnabled(boolean enabled) {
         VERSION_CHECK_ENABLED = enabled;
@@ -99,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;
     }
@@ -207,6 +215,9 @@ public class PhotonCamera {
     private void verifyVersion() {
         if (!VERSION_CHECK_ENABLED) return;
 
+        if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return;
+        lastVersionCheckTime = Timer.getFPGATimestamp();
+
         String versionString = versionEntry.getString("");
         if (versionString.equals("")) {
             DriverStation.reportError(

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);
@@ -144,7 +146,7 @@ public class SimPhotonCamera extends PhotonCamera {
             targetAreaEntry.setDouble(bestTarget.getArea());
             targetSkewEntry.setDouble(bestTarget.getSkew());
 
-            var transform = bestTarget.getCameraToTarget();
+            var transform = bestTarget.getBestCameraToTarget();
             double[] poseData = {
                 transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees()
             };

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

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

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

@@ -25,11 +25,15 @@
 #include "photonlib/PhotonCamera.h"
 
 #include <frc/Errors.h>
+#include <frc/Timer.h>
 
 #include "PhotonVersion.h"
 #include "photonlib/Packet.h"
 
 namespace photonlib {
+
+constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s;
+
 PhotonCamera::PhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
                            const std::string& cameraName)
     : mainTable(instance->GetTable("photonvision")),
@@ -48,7 +52,7 @@ PhotonCamera::PhotonCamera(const std::string& cameraName)
                        nt::NetworkTableInstance::GetDefault()),
                    cameraName) {}
 
-PhotonPipelineResult PhotonCamera::GetLatestResult() const {
+PhotonPipelineResult PhotonCamera::GetLatestResult() {
   // Prints warning if not connected
   VerifyVersion();
 
@@ -68,6 +72,10 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() const {
   photonlib::Packet packet{bytes};
 
   packet >> result;
+
+  result.SetTimestamp(units::microsecond_t(rawBytesEntry.GetLastChange()) -
+                      result.GetLatency());
+
   return result;
 }
 
@@ -99,9 +107,14 @@ void PhotonCamera::SetLEDMode(LEDMode mode) {
   ledModeEntry.SetDouble(static_cast<double>(static_cast<int>(mode)));
 }
 
-void PhotonCamera::VerifyVersion() const {
+void PhotonCamera::VerifyVersion() {
   if (!PhotonCamera::VERSION_CHECK_ENABLED) return;
 
+  if ((frc::Timer::GetFPGATimestamp() - lastVersionCheckTime) <
+      VERSION_CHECK_INTERVAL)
+    return;
+  this->lastVersionCheckTime = frc::Timer::GetFPGATimestamp();
+
   const std::string& versionString = versionEntry.GetString("");
   if (versionString.empty()) {
     std::string path_ = path;

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

@@ -41,12 +41,12 @@ PhotonTrackedTarget::PhotonTrackedTarget(
       area(area),
       skew(skew),
       fiducialId(id),
-      cameraToTarget(pose),
+      bestCameraToTarget(pose),
       corners(corners) {}
 
 bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
   return other.yaw == yaw && other.pitch == pitch && other.area == area &&
-         other.skew == skew && other.cameraToTarget == cameraToTarget &&
+         other.skew == skew && other.bestCameraToTarget == bestCameraToTarget &&
          other.corners == corners;
 }
 
@@ -56,13 +56,22 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
 
 Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
   packet << target.yaw << target.pitch << target.area << target.skew
-         << target.fiducialId << target.cameraToTarget.Translation().X().value()
-         << target.cameraToTarget.Translation().Y().value()
-         << target.cameraToTarget.Translation().Z().value()
-         << target.cameraToTarget.Rotation().GetQuaternion().W()
-         << target.cameraToTarget.Rotation().GetQuaternion().X()
-         << target.cameraToTarget.Rotation().GetQuaternion().Y()
-         << target.cameraToTarget.Rotation().GetQuaternion().Z();
+         << target.fiducialId
+         << target.bestCameraToTarget.Translation().X().value()
+         << target.bestCameraToTarget.Translation().Y().value()
+         << target.bestCameraToTarget.Translation().Z().value()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().W()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().X()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().Y()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().Z()
+         << target.altCameraToTarget.Translation().X().value()
+         << target.altCameraToTarget.Translation().Y().value()
+         << target.altCameraToTarget.Translation().Z().value()
+         << target.altCameraToTarget.Rotation().GetQuaternion().W()
+         << target.altCameraToTarget.Rotation().GetQuaternion().X()
+         << target.altCameraToTarget.Rotation().GetQuaternion().Y()
+         << target.altCameraToTarget.Rotation().GetQuaternion().Z()
+         << target.poseAmbiguity;
 
   for (int i = 0; i < 4; i++) {
     packet << target.corners[i].first << target.corners[i].second;
@@ -74,17 +83,30 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
 Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
   packet >> target.yaw >> target.pitch >> target.area >> target.skew >>
       target.fiducialId;
+
+  // We use these for best and alt transforms below
   double x = 0;
   double y = 0;
   double z = 0;
   double w = 0;
+
+  // First transform is the "best" pose
   packet >> x >> y >> z;
-  const auto translation = frc::Translation3d(
+  const auto bestTranslation = frc::Translation3d(
       units::meter_t(x), units::meter_t(y), units::meter_t(z));
   packet >> w >> x >> y >> z;
-  const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+  const auto bestRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+  target.bestCameraToTarget = frc::Transform3d(bestTranslation, bestRotation);
 
-  target.cameraToTarget = frc::Transform3d(translation, rotation);
+  // Second transform is the "alternate" pose
+  packet >> x >> y >> z;
+  const auto altTranslation = frc::Translation3d(
+      units::meter_t(x), units::meter_t(y), units::meter_t(z));
+  packet >> w >> x >> y >> z;
+  const auto altRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+  target.altCameraToTarget = frc::Transform3d(altTranslation, altRotation);
+
+  packet >> target.poseAmbiguity;
 
   target.corners.clear();
   for (int i = 0; i < 4; i++) {

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

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

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

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

@@ -30,6 +30,7 @@
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableEntry.h>
 #include <networktables/NetworkTableInstance.h>
+#include <units/time.h>
 #include <wpi/deprecated.h>
 
 #include "photonlib/PhotonPipelineResult.h"
@@ -66,7 +67,7 @@ class PhotonCamera {
    * Returns the latest pipeline result.
    * @return The latest pipeline result.
    */
-  PhotonPipelineResult GetLatestResult() const;
+  PhotonPipelineResult GetLatestResult();
 
   /**
    * Toggles driver mode.
@@ -136,7 +137,7 @@ class PhotonCamera {
    */
   WPI_DEPRECATED(
       "This method should be replaced with PhotonPipelineResult::HasTargets()")
-  bool HasTargets() const { return GetLatestResult().HasTargets(); }
+  bool HasTargets() { return GetLatestResult().HasTargets(); }
 
   inline static void SetVersionCheckEnabled(bool enabled) {
     PhotonCamera::VERSION_CHECK_ENABLED = enabled;
@@ -158,9 +159,10 @@ class PhotonCamera {
   mutable Packet packet;
 
  private:
+  units::second_t lastVersionCheckTime = 0_s;
   inline static bool VERSION_CHECK_ENABLED = true;
 
-  void VerifyVersion() const;
+  void VerifyVersion();
 };
 
 }  // namespace photonlib

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

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

@@ -91,10 +91,28 @@ class PhotonTrackedTarget {
   }
 
   /**
-   * Returns the pose of the target relative to the robot.
+   * Get the ratio of pose reprojection errors, called ambiguity. Numbers above
+   * 0.2 are likely to be ambiguous. -1 if invalid.
+   */
+  double GetPoseAmbiguity() const { return poseAmbiguity; }
+
+  /**
+   * Get the transform that maps camera space (X = forward, Y = left, Z = up) to
+   * object/fiducial tag space (X forward, Y left, Z up) with the lowest
+   * reprojection error. The ratio between this and the alternate target's
+   * reprojection error is the ambiguity, which is between 0 and 1.
    * @return The pose of the target relative to the robot.
    */
-  frc::Transform3d GetCameraToTarget() const { return cameraToTarget; }
+  frc::Transform3d GetBestCameraToTarget() const { return bestCameraToTarget; }
+
+  /**
+   * Get the transform that maps camera space (X = forward, Y = left, Z = up) to
+   * object/fiducial tag space (X forward, Y left, Z up) with the highest
+   * reprojection error
+   */
+  frc::Transform3d GetAlternateCameraToTarget() const {
+    return altCameraToTarget;
+  }
 
   bool operator==(const PhotonTrackedTarget& other) const;
   bool operator!=(const PhotonTrackedTarget& other) const;
@@ -108,7 +126,9 @@ class PhotonTrackedTarget {
   double area = 0;
   double skew = 0;
   int fiducialId;
-  frc::Transform3d cameraToTarget;
+  frc::Transform3d bestCameraToTarget;
+  frc::Transform3d altCameraToTarget;
+  double poseAmbiguity;
   wpi::SmallVector<std::pair<double, double>, 4> corners;
 };
 }  // namespace photonlib

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

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

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

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

@@ -45,6 +45,8 @@ class PacketTest {
                         -5.0,
                         -1,
                         new Transform3d(new Translation3d(), new Rotation3d()),
+                        new Transform3d(new Translation3d(), new Rotation3d()),
+                        0.25,
                         List.of(
                                 new TargetCorner(1, 2),
                                 new TargetCorner(3, 4),
@@ -81,6 +83,8 @@ class PacketTest {
                                         4.0,
                                         2,
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        0.25,
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -93,6 +97,8 @@ class PacketTest {
                                         6.7,
                                         3,
                                         new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
+                                        new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
+                                        0.25,
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),

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

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

photon-server/build.gradle

@@ -13,6 +13,7 @@ apply from: "${rootDir}/shared/common.gradle"
 
 dependencies {
     implementation project(':photon-core')
+    implementation project(':photon-targeting')
 
     implementation "io.javalin:javalin:4.2.0"
 
@@ -42,9 +43,16 @@ task copyClientUIToResources(type: Copy) {
     into "${projectDir}/src/main/resources/web/"
 }
 
+task copyThinclientToResources(type: Copy) {
+    from "${projectDir}/../photon-thinclient/"
+    into "${projectDir}/src/main/resources/web/"
+}
+
+
 task buildAndCopyUI {}
 
 buildAndCopyUI.dependsOn copyClientUIToResources
+buildAndCopyUI.dependsOn copyThinclientToResources
 copyClientUIToResources.dependsOn runNpmOnClient
 copyClientUIToResources.shouldRunAfter runNpmOnClient
 

photon-server/src/main/java/org/photonvision/server/CameraSocketHandler.java

@@ -0,0 +1,140 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.server;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import io.javalin.websocket.WsBinaryMessageContext;
+import io.javalin.websocket.WsCloseContext;
+import io.javalin.websocket.WsConnectContext;
+import io.javalin.websocket.WsContext;
+import io.javalin.websocket.WsMessageContext;
+import java.util.HashMap;
+import java.util.List;
+import java.util.concurrent.CopyOnWriteArrayList;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.vision.videoStream.SocketVideoStreamManager;
+
+public class CameraSocketHandler {
+    private final Logger logger = new Logger(CameraSocketHandler.class, LogGroup.WebServer);
+    private final List<WsContext> users = new CopyOnWriteArrayList<>();
+    private final SocketVideoStreamManager svsManager = SocketVideoStreamManager.getInstance();
+
+    private Thread cameraBroadcastThread;
+
+    public static class UIMap extends HashMap<String, Object> {}
+
+    private static class ThreadSafeSingleton {
+        private static final CameraSocketHandler INSTANCE = new CameraSocketHandler();
+    }
+
+    public static CameraSocketHandler getInstance() {
+        return CameraSocketHandler.ThreadSafeSingleton.INSTANCE;
+    }
+
+    private CameraSocketHandler() {
+        cameraBroadcastThread = new Thread(this::broadcastFramesTask);
+        cameraBroadcastThread.setPriority(2); // fairly low priority
+        cameraBroadcastThread.start();
+    }
+
+    public void onConnect(WsConnectContext context) {
+        context.session.setIdleTimeout(Long.MAX_VALUE); // TODO: determine better value
+        var insa = context.session.getRemote().getInetSocketAddress();
+        var host = insa.getAddress().toString() + ":" + insa.getPort();
+        logger.info("New camera websocket connection from " + host);
+        users.add(context);
+    }
+
+    protected void onClose(WsCloseContext context) {
+        var insa = context.session.getRemote().getInetSocketAddress();
+        var host = insa.getAddress().toString() + ":" + insa.getPort();
+        var reason = context.reason() != null ? context.reason() : "Connection closed by client";
+        logger.info("Closing camera websocket connection from " + host + " for reason: " + reason);
+        svsManager.removeSubscription(context);
+        users.remove(context);
+    }
+
+    @SuppressWarnings({"unchecked"})
+    public void onMessage(WsMessageContext context) {
+        var messageStr = context.message();
+        ObjectMapper mapper = new ObjectMapper();
+        try {
+            JsonNode actualObj = mapper.readTree(messageStr);
+
+            try {
+                var entryCmd = actualObj.get("cmd").asText();
+                var socketMessageType = CameraSocketMessageType.fromEntryKey(entryCmd);
+
+                logger.trace(() -> "Got Camera WS message: [" + socketMessageType + "]");
+
+                if (socketMessageType == null) {
+                    logger.warn("Got unknown socket message command: " + entryCmd);
+                }
+
+                switch (socketMessageType) {
+                    case CSMT_SUBSCRIBE:
+                        {
+                            int portId = actualObj.get("port").asInt();
+                            svsManager.addSubscription(context, portId);
+                            break;
+                        }
+                    case CSMT_UNSUBSCRIBE:
+                        {
+                            svsManager.removeSubscription(context);
+                            break;
+                        }
+                }
+            } catch (Exception e) {
+                logger.error("Failed to parse message!", e);
+            }
+
+        } catch (JsonProcessingException e) {
+            logger.warn("Could not parse message \"" + messageStr + "\"");
+            e.printStackTrace();
+            return;
+        }
+    }
+
+    @SuppressWarnings({"unchecked"})
+    public void onBinaryMessage(WsBinaryMessageContext context) {
+        return; // ignoring binary messages for now
+    }
+
+    private void broadcastFramesTask() {
+        // Background camera image broadcasting thread
+        while (!Thread.currentThread().isInterrupted()) {
+            svsManager.allStreamConvertNextFrame();
+
+            try {
+                Thread.sleep(1);
+            } catch (InterruptedException e) {
+                logger.error("Exception waiting for camera stream broadcast semaphore", e);
+            }
+
+            for (var user : users) {
+                var sendBytes = svsManager.getSendFrame(user);
+                if (sendBytes != null) {
+                    user.send(sendBytes);
+                }
+            }
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/server/CameraSocketMessageType.java

@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.server;
+
+import java.util.EnumSet;
+import java.util.HashMap;
+import java.util.Map;
+
+@SuppressWarnings("unused")
+public enum CameraSocketMessageType {
+    CSMT_SUBSCRIBE("subscribe"),
+    CSMT_UNSUBSCRIBE("unsubscribe");
+
+    public final String entryKey;
+
+    CameraSocketMessageType(String entryKey) {
+        this.entryKey = entryKey;
+    }
+
+    private static final Map<String, CameraSocketMessageType> entryKeyToValueMap = new HashMap<>();
+
+    static {
+        for (var value : EnumSet.allOf(CameraSocketMessageType.class)) {
+            entryKeyToValueMap.put(value.entryKey, value);
+        }
+    }
+
+    public static CameraSocketMessageType fromEntryKey(String entryKey) {
+        return entryKeyToValueMap.get(entryKey);
+    }
+}

photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java

@@ -42,8 +42,8 @@ import org.photonvision.common.logging.Logger;
 import org.photonvision.vision.pipeline.PipelineType;
 
 @SuppressWarnings("rawtypes")
-public class SocketHandler {
-    private final Logger logger = new Logger(SocketHandler.class, LogGroup.WebServer);
+public class DataSocketHandler {
+    private final Logger logger = new Logger(DataSocketHandler.class, LogGroup.WebServer);
     private final List<WsContext> users = new CopyOnWriteArrayList<>();
     private final ObjectMapper objectMapper = new ObjectMapper(new MessagePackFactory());
     private final DataChangeService dcService = DataChangeService.getInstance();
@@ -54,14 +54,14 @@ public class SocketHandler {
     public static class UIMap extends HashMap<String, Object> {}
 
     private static class ThreadSafeSingleton {
-        private static final SocketHandler INSTANCE = new SocketHandler();
+        private static final DataSocketHandler INSTANCE = new DataSocketHandler();
     }
 
-    public static SocketHandler getInstance() {
-        return SocketHandler.ThreadSafeSingleton.INSTANCE;
+    public static DataSocketHandler getInstance() {
+        return DataSocketHandler.ThreadSafeSingleton.INSTANCE;
     }
 
-    private SocketHandler() {
+    private DataSocketHandler() {
         dcService.addSubscribers(
                 uiOutboundSubscriber,
                 new UIInboundSubscriber()); // Subscribe outgoing messages to the data change service
@@ -84,19 +84,6 @@ public class SocketHandler {
         var reason = context.reason() != null ? context.reason() : "Connection closed by client";
         logger.info("Closing websocket connection from " + host + " for reason: " + reason);
         users.remove(context);
-
-        if (users.size() == 0) {
-            logger.info("All websocket connections are closed. Setting inputShouldShow to false.");
-
-            // cameraIndex -1 means the event is received by all cameras
-            dcService.publishEvent(
-                    new IncomingWebSocketEvent<>(
-                            DataChangeDestination.DCD_ACTIVEPIPELINESETTINGS,
-                            "inputShouldShow",
-                            false,
-                            -1,
-                            null));
-        }
     }
 
     @SuppressWarnings({"unchecked"})
@@ -117,7 +104,7 @@ public class SocketHandler {
                 try {
                     var entryKey = entry.getKey();
                     var entryValue = entry.getValue();
-                    var socketMessageType = SocketMessageType.fromEntryKey(entryKey);
+                    var socketMessageType = DataSocketMessageType.fromEntryKey(entryKey);
 
                     logger.trace(
                             () ->

photon-server/src/main/java/org/photonvision/server/DataSocketMessageType.java

@@ -22,7 +22,7 @@ import java.util.HashMap;
 import java.util.Map;
 
 @SuppressWarnings("unused")
-public enum SocketMessageType {
+public enum DataSocketMessageType {
     SMT_DRIVERMODE("driverMode"),
     SMT_CHANGECAMERANAME("changeCameraName"),
     SMT_CHANGEPIPELINENAME("changePipelineName"),
@@ -40,19 +40,19 @@ public enum SocketMessageType {
 
     public final String entryKey;
 
-    SocketMessageType(String entryKey) {
+    DataSocketMessageType(String entryKey) {
         this.entryKey = entryKey;
     }
 
-    private static final Map<String, SocketMessageType> entryKeyToValueMap = new HashMap<>();
+    private static final Map<String, DataSocketMessageType> entryKeyToValueMap = new HashMap<>();
 
     static {
-        for (var value : EnumSet.allOf(SocketMessageType.class)) {
+        for (var value : EnumSet.allOf(DataSocketMessageType.class)) {
             entryKeyToValueMap.put(value.entryKey, value);
         }
     }
 
-    public static SocketMessageType fromEntryKey(String entryKey) {
+    public static DataSocketMessageType fromEntryKey(String entryKey) {
         return entryKeyToValueMap.get(entryKey);
     }
 }

photon-server/src/main/java/org/photonvision/server/RequestHandler.java

@@ -19,7 +19,6 @@ package org.photonvision.server;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.databind.ObjectMapper;
-import edu.wpi.first.math.geometry.Rotation2d;
 import io.javalin.http.Context;
 import java.io.File;
 import java.io.FileInputStream;
@@ -173,16 +172,12 @@ public class RequestHandler {
             var settings = (HashMap<String, Object>) settingsAndIndex.get("settings");
             int index = (Integer) settingsAndIndex.get("index");
 
-            // The only settings we actually care about are FOV and pitch
+            // The only settings we actually care about are FOV
             var fov = Double.parseDouble(settings.get("fov").toString());
-            var pitch =
-                    Rotation2d.fromDegrees(Double.parseDouble(settings.get("tiltDegrees").toString()));
 
-            logger.info(
-                    String.format(
-                            "Setting camera %s's fov to %s w/pitch %s", index, fov, pitch.getDegrees()));
+            logger.info(String.format("Setting camera %s's fov to %s", index, fov));
             var module = VisionModuleManager.getInstance().getModule(index);
-            module.setFovAndPitch(fov, pitch);
+            module.setFov(fov);
             module.saveModule();
         } catch (JsonProcessingException e) {
             logger.error("Got invalid camera setting JSON from frontend!");

photon-server/src/main/java/org/photonvision/server/Server.java

@@ -61,15 +61,24 @@ public class Server {
                                                                     })));
                         });
 
-        var socketHandler = SocketHandler.getInstance();
-
-        /*Web Socket Events */
+        /*Web Socket Events for Data Exchage */
+        var dsHandler = DataSocketHandler.getInstance();
         app.ws(
-                "/websocket",
+                "/websocket_data",
                 ws -> {
-                    ws.onConnect(socketHandler::onConnect);
-                    ws.onClose(socketHandler::onClose);
-                    ws.onBinaryMessage(socketHandler::onBinaryMessage);
+                    ws.onConnect(dsHandler::onConnect);
+                    ws.onClose(dsHandler::onClose);
+                    ws.onBinaryMessage(dsHandler::onBinaryMessage);
+                });
+        /*Web Socket Events for Camera Streaming */
+        var camDsHandler = CameraSocketHandler.getInstance();
+        app.ws(
+                "/websocket_cameras",
+                ws -> {
+                    ws.onConnect(camDsHandler::onConnect);
+                    ws.onClose(camDsHandler::onClose);
+                    ws.onBinaryMessage(camDsHandler::onBinaryMessage);
+                    ws.onMessage(camDsHandler::onMessage);
                 });
         /*API Events*/
         app.post("/api/settings/import", RequestHandler::onSettingUpload);

photon-server/src/main/java/org/photonvision/server/UIOutboundSubscriber.java

@@ -35,9 +35,9 @@ import org.photonvision.common.logging.Logger;
 class UIOutboundSubscriber extends DataChangeSubscriber {
     Logger logger = new Logger(UIOutboundSubscriber.class, LogGroup.WebServer);
 
-    private final SocketHandler socketHandler;
+    private final DataSocketHandler socketHandler;
 
-    public UIOutboundSubscriber(SocketHandler socketHandler) {
+    public UIOutboundSubscriber(DataSocketHandler socketHandler) {
         super(DataChangeSource.AllSources, Collections.singletonList(DataChangeDestination.DCD_UI));
         this.socketHandler = socketHandler;
     }

photon-server/src/main/resources/web/docs/index.html

@@ -1 +0,0 @@
-<p>Docs have not been copied!</p>

photon-server/src/main/resources/web/index.html

@@ -1 +1 @@
-<p>UI has not been copied!</p>
+<p>UI has not been copied!</p>

photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java

@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import java.io.IOException;
+import java.util.stream.Collectors;
+import org.junit.jupiter.api.Assertions;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.photonvision.common.util.TestUtils;
+import org.photonvision.vision.apriltag.AprilTagJNI;
+import org.photonvision.vision.camera.QuirkyCamera;
+import org.photonvision.vision.frame.provider.FileFrameProvider;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TargetModel;
+import org.photonvision.vision.target.TrackedTarget;
+
+public class AprilTagTest {
+    @BeforeEach
+    public void Init() throws IOException {
+        TestUtils.loadLibraries();
+        AprilTagJNI.forceLoad();
+    }
+
+    @Test
+    public void testApriltagFacingCamera() {
+        var pipeline = new AprilTagPipeline();
+
+        pipeline.getSettings().inputShouldShow = true;
+        pipeline.getSettings().outputShouldDraw = true;
+        pipeline.getSettings().solvePNPEnabled = true;
+        pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
+        pipeline.getSettings().cornerDetectionUseConvexHulls = true;
+        pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_640_480, false),
+                        TestUtils.WPI2020Image.FOV,
+                        TestUtils.get2020LifeCamCoeffs(false));
+
+        CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+        printTestResults(pipelineResult);
+
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        outputPipe.process(
+                pipelineResult.inputFrame,
+                pipelineResult.outputFrame,
+                pipeline.getSettings(),
+                pipelineResult.targets);
+
+        TestUtils.showImage(pipelineResult.inputFrame.image.getMat(), "Pipeline output", 999999);
+
+        // these numbers are not *accurate*, but they are known and expected
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
+        Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
+        Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
+        Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
+
+        var objX = new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY();
+        var objY = new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ();
+        var objZ = new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX();
+        System.out.printf("Object x %.2f y %.2f z %.2f\n", objX, objY, objZ);
+
+        // We expect the object X to be forward, or -X in world space
+        Assertions.assertEquals(
+                -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.1);
+        // We expect the object Y axis to be right, or negative-Y in world space
+        Assertions.assertEquals(
+                -1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.1);
+        // We expect the object Z axis to be up, or +Z in world space
+        Assertions.assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.1);
+    }
+
+    private static void printTestResults(CVPipelineResult pipelineResult) {
+        double fps = 1000 / pipelineResult.getLatencyMillis();
+        System.out.println(
+                "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)");
+        System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
+        System.out.println(
+                "Found targets at "
+                        + pipelineResult.targets.stream()
+                                .map(TrackedTarget::getBestCameraToTarget3d)
+                                .collect(Collectors.toList()));
+    }
+}

photon-targeting/build.gradle

@@ -1,5 +1,7 @@
 apply plugin: "java"
 
+apply from: "${rootDir}/shared/common.gradle"
+
 sourceCompatibility = 11
 targetCompatibility = 11
 

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