3d viewer cleanup (#490)

* WIP fiddling with 3js stuff for different viewpoints

* more wip viewer cleanup

* More cleanups - split out minimap

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/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,6 +86,95 @@ 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)
@@ -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/store/index.js

@@ -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,8 @@ export default new Vuex.Store({
                     blur: 0.0,
                     threads: 1,
                     debug: false,
-                    refineEdges: true
+                    refineEdges: true,
+                    numIterations: 1,
                 }
             }
         ],

photon-client/src/views/CamerasView.vue

@@ -146,6 +146,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 +176,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>
@@ -396,6 +417,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);
                     }

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

@@ -12,40 +12,55 @@
     <CVslider
       v-model="decimate"
       class="pt-2"
-      slider-cols="12"
+      slider-cols="8"
       name="Decimate"
       min="0"
       max="3"
       step=".5"
+      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="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>
 
@@ -82,6 +97,14 @@
                 this.$store.commit("mutatePipeline", {"decimate": 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,7 +1,7 @@
 <template>
   <div>
     <CVslider
-      v-if="cameraExposure !== -1"
+      :disabled="cameraAutoExposure"
       v-model="cameraExposure"
       name="Exposure"
       min="0"
@@ -22,10 +22,27 @@
       @input="handlePipelineData('cameraBrightness')"
       @rollback="e => rollback('cameraBrightness', e)"
     />
+    <CVswitch
+      v-model="cameraAutoExposure"
+      class="pt-2"
+      name="Auto exposure"
+      @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('cameraRedGain')"
+      @rollback="e => rollback('cameraRedGain', 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 +53,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 +93,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,6 +102,7 @@
         components: {
             CVslider,
             CVselect,
+            CVswitch,
         },
         // eslint-disable-next-line vue/require-prop-types
         props: ['value'],
@@ -109,6 +128,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)

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,14 +44,12 @@
 
 <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 {
@@ -87,21 +80,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-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -233,6 +233,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
                             t.getSkew(),
                             t.getFiducialId(),
                             t.getCameraToTarget3d(),
+                            t.getPoseAmbiguity(),
                             cornerList));
         }
         return ret;

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

@@ -19,14 +19,17 @@ package org.photonvision.common.util.math;
 
 import edu.wpi.first.math.MatBuilder;
 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.util.Units;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.Arrays;
 import java.util.List;
+import org.opencv.core.Mat;
 
 public class MathUtils {
     MathUtils() {}
@@ -159,25 +162,33 @@ public class MathUtils {
     // TODO: Refactor into new pipe?
     public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
         // CameraToTarget _should_ be in opencv-land EDN
-
-        var pose =
-                CoordinateSystem.convert(
-                        new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
-
-        return pose;
+        return CoordinateSystem.convert(
+                new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
     }
 
-    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());
     }
 }

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

@@ -19,8 +19,8 @@ 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;
 
@@ -81,11 +81,11 @@ public class DetectionResult {
         return error2;
     }
 
-    public Pose3d getPoseResult1() {
+    public Transform3d getPoseResult1() {
         return poseResult1;
     }
 
-    public Pose3d getPoseResult2() {
+    public Transform3d getPoseResult2() {
         return poseResult2;
     }
 
@@ -96,9 +96,9 @@ public class DetectionResult {
     double centerX, centerY;
     double[] corners;
 
-    Pose3d poseResult1;
+    Transform3d poseResult1;
     double error1;
-    Pose3d poseResult2;
+    Transform3d poseResult2;
     double error2;
 
     public DetectionResult(
@@ -125,16 +125,31 @@ 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)));
         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)));
     }
 
+    /**
+     * 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
     public String toString() {
         return "DetectionResult [centerX="

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

@@ -31,7 +31,8 @@ public class QuirkyCamera {
                     new QuirkyCamera(0x825, 0x46D, CameraQuirk.CompletelyBroken), // Logitec C270
                     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,21 @@ 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);
         }
     }
 
-    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 +97,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 +168,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
@@ -127,8 +134,51 @@ public class Draw3dTargetsPipe
                             ColorHelper.colorToScalar(Color.orange),
                             3);
                 }
+
+                // Draw X, Y and Z axis
+                MatOfPoint3f pointMat = new MatOfPoint3f();
+                var list =
+                        List.of(
+                                new Point3(0, 0, 0),
+                                new Point3(0.2, 0, 0),
+                                new Point3(0, 0.2, 0),
+                                new Point3(0, 0, 0.2));
+                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(1),
+                        ColorHelper.colorToScalar(Color.RED),
+                        3);
+                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);
+
                 tempMat.release();
                 jac.release();
+                pointMat.release();
             }
 
             // draw corners

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

@@ -117,9 +117,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,7 +131,6 @@ 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<>();
@@ -140,7 +143,7 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
                             new TargetCalculationParameters(
                                     false, null, null, null, null, frameStaticProperties));
 
-            var correctedPose = MathUtils.convertApriltagtoPhotonPose(target.getCameraToTarget3d());
+            var correctedPose = MathUtils.convertOpenCVtoPhotonPose(target.getCameraToTarget3d());
             target.setCameraToTarget3d(
                     new Transform3d(correctedPose.getTranslation(), correctedPose.getRotation()));
 
@@ -150,11 +153,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

@@ -40,6 +40,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("Creatig Reflective pipeline");
                     currentUserPipeline =
                             new ReflectivePipeline((ReflectivePipelineSettings) desiredPipelineSettings);
                     break;
                 case ColoredShape:
+                    logger.debug("Creatig ColoredShape pipeline");
                     currentUserPipeline =
                             new ColoredShapePipeline((ColoredShapePipelineSettings) desiredPipelineSettings);
                     break;
                 case AprilTag:
+                    logger.debug("Creatig 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

@@ -44,7 +44,6 @@ 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;
@@ -361,7 +360,7 @@ public class VisionModule {
             settings.cameraBlueGain = -1;
         }
 
-        settings.cameraExposure = -1;
+        settings.cameraAutoExposure = true;
 
         setPipeline(PipelineManager.CAL_3D_INDEX);
     }
@@ -400,22 +399,14 @@ 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().setAutoExposure(pipelineSettings.cameraAutoExposure);
 
         if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
             // If the gain is disabled for some reason, re-enable it

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,7 @@ 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));
 
     @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.*;
@@ -51,6 +51,7 @@ public class TrackedTarget implements Releasable {
     private CVShape m_shape;
 
     private int m_fiducialId = -1;
+    private double m_poseAmbiguity = -1;
 
     private Mat m_cameraRelativeTvec, m_cameraRelativeRvec;
 
@@ -72,14 +73,16 @@ public class TrackedTarget implements Releasable {
         m_yaw =
                 TargetCalculations.calculateYaw(
                         result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
-        Pose3d bestPose = new Pose3d();
+        var bestPose = new Transform3d();
         if (result.getError1() <= result.getError2()) {
             bestPose = result.getPoseResult1();
         } else {
             bestPose = result.getPoseResult2();
         }
 
-        m_cameraToTarget3d = new Transform3d(new Pose3d(), bestPose);
+        bestPose = MathUtils.convertApriltagtoOpenCV(bestPose);
+
+        m_cameraToTarget3d = bestPose;
 
         double[] corners = result.getCorners();
         Point[] cornerPoints =
@@ -113,10 +116,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 +130,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.
      *
@@ -260,6 +271,7 @@ public class TrackedTarget implements Releasable {
         ret.put("yaw", getYaw());
         ret.put("skew", getSkew());
         ret.put("area", getArea());
+        ret.put("ambiguity", getPoseAmbiguity());
         if (getCameraToTarget3d() != null) {
             ret.put("pose", transformToMap(getCameraToTarget3d()));
         }

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/SolvePNPTest.java

@@ -150,6 +150,14 @@ 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();
         Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05);

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/src/main/java/org/photonvision/SimVisionSystem.java

@@ -171,6 +171,7 @@ public class SimVisionSystem {
                                         0.0,
                                         -1, // TODO fiducial ID
                                         new Transform3d(),
+                                        0.25,
                                         List.of(
                                                 new TargetCorner(0, 0), new TargetCorner(0, 0),
                                                 new TargetCorner(0, 0), new TargetCorner(0, 0))));

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

@@ -62,7 +62,8 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
          << target.cameraToTarget.Rotation().GetQuaternion().W()
          << target.cameraToTarget.Rotation().GetQuaternion().X()
          << target.cameraToTarget.Rotation().GetQuaternion().Y()
-         << target.cameraToTarget.Rotation().GetQuaternion().Z();
+         << target.cameraToTarget.Rotation().GetQuaternion().Z()
+         << target.poseAmbiguity;
 
   for (int i = 0; i < 4; i++) {
     packet << target.corners[i].first << target.corners[i].second;
@@ -86,6 +87,8 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
 
   target.cameraToTarget = frc::Transform3d(translation, rotation);
 
+  packet >> target.poseAmbiguity;
+
   target.corners.clear();
   for (int i = 0; i < 4; i++) {
     double first = 0;

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

@@ -90,6 +90,12 @@ class PhotonTrackedTarget {
     return corners;
   }
 
+  /**
+   * 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; }
+
   /**
    * Returns the pose of the target relative to the robot.
    * @return The pose of the target relative to the robot.
@@ -109,6 +115,7 @@ class PhotonTrackedTarget {
   double skew = 0;
   int fiducialId;
   frc::Transform3d cameraToTarget;
+  double poseAmbiguity;
   wpi::SmallVector<std::pair<double, double>, 4> corners;
 };
 }  // namespace photonlib

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

@@ -45,6 +45,7 @@ class PacketTest {
                         -5.0,
                         -1,
                         new Transform3d(new Translation3d(), new Rotation3d()),
+                        0.25,
                         List.of(
                                 new TargetCorner(1, 2),
                                 new TargetCorner(3, 4),
@@ -81,6 +82,7 @@ class PacketTest {
                                         4.0,
                                         2,
                                         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 +95,7 @@ class PacketTest {
                                         6.7,
                                         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-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"
 

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

@@ -1 +1 @@
-<p>UI has not been copied!</p>
+<!DOCTYPE html><html lang=en><head><meta charset=utf-8><meta http-equiv=X-UA-Compatible content="IE=edge"><meta name=viewport content="width=device-width,initial-scale=1"><link rel=icon href=/favicon.png><title>PhotonVision</title><link href=/js/chunk-2d216214.4302abb0.js rel=prefetch><link href=/js/chunk-2d216257.0de4b8cc.js rel=prefetch><link href=/js/chunk-ad0384ec.7dafafc8.js rel=prefetch><link href=/css/app.a6eec7e2.css rel=preload as=style><link href=/css/chunk-vendors.db45c662.css rel=preload as=style><link href=/js/app.612f1f56.js rel=preload as=script><link href=/js/chunk-vendors.58a768b6.js rel=preload as=script><link href=/css/chunk-vendors.db45c662.css rel=stylesheet><link href=/css/app.a6eec7e2.css rel=stylesheet></head><body><noscript><strong>We're sorry but PhotonVision doesn't work properly without JavaScript enabled. Please enable it to continue.</strong></noscript><div id=app></div><script src=/js/chunk-vendors.58a768b6.js></script><script src=/js/app.612f1f56.js></script></body></html>

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

@@ -0,0 +1,104 @@
+/*
+ * 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).getCameraToTarget3d();
+        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 axis to be to the right, or negative-Y in world space
+        Assertions.assertEquals(
+                -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY(), 0.08);
+        // We expect the object Y axis to be up, or +Z in world space
+        Assertions.assertEquals(
+                1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ(), 0.08);
+        // We expect the object Z axis to towards the camera, or negative-X in world space
+        Assertions.assertEquals(
+                -1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX(), 0.08);
+    }
+
+    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::getCameraToTarget3d)
+                                .collect(Collectors.toList()));
+    }
+}

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

@@ -27,7 +27,7 @@ import java.util.Objects;
 import org.photonvision.common.dataflow.structures.Packet;
 
 public class PhotonTrackedTarget {
-    public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4);
+    public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4 + 1);
 
     private double yaw;
     private double pitch;
@@ -35,6 +35,7 @@ public class PhotonTrackedTarget {
     private double skew;
     private int fiducialId;
     private Transform3d cameraToTarget = new Transform3d();
+    private double poseAmbiguity;
     private List<TargetCorner> targetCorners;
 
     public PhotonTrackedTarget() {}
@@ -47,6 +48,7 @@ public class PhotonTrackedTarget {
             double skew,
             int id,
             Transform3d pose,
+            double ambiguity,
             List<TargetCorner> corners) {
         assert corners.size() == 4;
         this.yaw = yaw;
@@ -56,6 +58,7 @@ public class PhotonTrackedTarget {
         this.fiducialId = id;
         this.cameraToTarget = pose;
         this.targetCorners = corners;
+        this.poseAmbiguity = ambiguity;
     }
 
     public double getYaw() {
@@ -79,6 +82,14 @@ public class PhotonTrackedTarget {
         return fiducialId;
     }
 
+    /**
+     * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
+     * ambiguous. -1 if invalid.
+     */
+    public double getPoseAmbiguity() {
+        return poseAmbiguity;
+    }
+
     /**
      * Return a list of the 4 corners in image space (origin top left, x left, y down), in no
      * particular order, of the minimum area bounding rectangle of this target
@@ -134,6 +145,8 @@ public class PhotonTrackedTarget {
         y = packet.decodeDouble();
         z = packet.decodeDouble();
         var rotation = new Rotation3d(new Quaternion(w, x, y, z));
+        this.cameraToTarget = new Transform3d(translation, rotation);
+        this.poseAmbiguity = packet.decodeDouble();
 
         this.targetCorners = new ArrayList<>(4);
         for (int i = 0; i < 4; i++) {
@@ -142,8 +155,6 @@ public class PhotonTrackedTarget {
             targetCorners.add(new TargetCorner(cx, cy));
         }
 
-        this.cameraToTarget = new Transform3d(translation, rotation);
-
         return packet;
     }
 
@@ -166,6 +177,7 @@ public class PhotonTrackedTarget {
         packet.encode(cameraToTarget.getRotation().getQuaternion().getX());
         packet.encode(cameraToTarget.getRotation().getQuaternion().getY());
         packet.encode(cameraToTarget.getRotation().getQuaternion().getZ());
+        packet.encode(poseAmbiguity);
 
         for (int i = 0; i < 4; i++) {
             packet.encode(targetCorners.get(i).x);