diff --git a/.github/workflows/main.yml b/.github/workflows/build.yml
similarity index 57%
rename from .github/workflows/main.yml
rename to .github/workflows/build.yml
index 94a5e5f075..9a72b229b2 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/build.yml
@@ -1,9 +1,5 @@
-# This workflow builds the client (UI), the server, builds the JAR.
+name: Build
 
-name: CI
-
-# Controls when the action will run. Triggers the workflow on push or pull request
-# events but only for the master branch
 on:
   push:
     branches: [ master ]
@@ -11,212 +7,117 @@ on:
       - 'v*'
   pull_request:
     branches: [ master ]
-  merge_group:
 
 jobs:
-  photonclient-build:
-    # Let all steps run within the photon-client dir.
+  build-client:
+    name: "PhotonClient Build"
     defaults:
       run:
         working-directory: photon-client
-
-    # The type of runner that the job will run on.
-    runs-on: ubuntu-22.04
-
-    steps:
-    # Checkout code.
-    - uses: actions/checkout@v3
-
-    # Setup Node.js
-    - name: Setup Node.js
-      uses: actions/setup-node@v3
-      with:
-        node-version: 17
-
-    # Run npm
-    - name: Install Dependencies
-      run: npm ci
-
-    - name: Build Production Client
-      run: npm run build
-
-    # Upload client artifact.
-    - uses: actions/upload-artifact@master
-      with:
-        name: built-client
-        path: photon-client/dist/
-  photonclient-check-lint:
-    # Let all steps run within the photon-client dir.
-    defaults:
-      run:
-        working-directory: photon-client
-
-    # The type of runner that the job will run on.
     runs-on: ubuntu-22.04
-
     steps:
-    # Checkout code.
-    - uses: actions/checkout@v3
-
-    # Setup Node.js
-    - name: Setup Node.js
-      uses: actions/setup-node@v3
-      with:
-        node-version: 17
-
-    - name: Install Dependencies
-      run: npm ci
-
-    - name: Check Linting
-      run: npm run lint-ci
-
-    - name: Check Formatting
-      run: npm run format-ci
-  photon-build-examples:
-    runs-on: ubuntu-22.04
+      - uses: actions/checkout@v3
+      - name: Setup Node.js
+        uses: actions/setup-node@v3
+        with:
+          node-version: 18
+      - name: Install Dependencies
+        run: npm ci
+      - name: Build Production Client
+        run: npm run build
+      - uses: actions/upload-artifact@master
+        with:
+          name: built-client
+          path: photon-client/dist/
+  build-examples:
     name: "Build Examples"
-
+    runs-on: ubuntu-22.04
     steps:
-      # Checkout code.
       - name: Checkout code
         uses: actions/checkout@v3
         with:
           fetch-depth: 0
-
-      # Fetch tags.
       - name: Fetch tags
         run: git fetch --tags --force
-
-      # Install Java 17.
       - name: Install Java 17
         uses: actions/setup-java@v3
         with:
           java-version: 17
           distribution: temurin
-
       # Need to publish to maven local first, so that C++ sim can pick it up
-      # Still haven't figure out how to make the vendordep file be copied before trying to build examples
+      # Still haven't figured out how to make the vendordep file be copied before trying to build examples
       - name: Publish photonlib to maven local
         run: |
           chmod +x gradlew
           ./gradlew publishtomavenlocal -x check
-
       - name: Build Java examples
         working-directory: photonlib-java-examples
         run: |
           chmod +x gradlew
           ./gradlew copyPhotonlib -x check
           ./gradlew build -x check --max-workers 2
-
       - name: Build C++ examples
         working-directory: photonlib-cpp-examples
         run: |
           chmod +x gradlew
           ./gradlew copyPhotonlib -x check
           ./gradlew build -x check --max-workers 2
-  photon-build-all:
-    # The type of runner that the job will run on.
+  build-gradle:
+    name: "Gradle Build"
     runs-on: ubuntu-22.04
-
     steps:
       # Checkout code.
       - name: Checkout code
         uses: actions/checkout@v3
         with:
           fetch-depth: 0
-
-      # Fetch tags.
       - name: Fetch tags
         run: git fetch --tags --force
-
-      # Install Java 17.
       - name: Install Java 17
         uses: actions/setup-java@v3
         with:
           java-version: 17
           distribution: temurin
-
-      # Run only build tasks, no checks??
       - name: Gradle Build
         run: |
           chmod +x gradlew
           ./gradlew photon-server:build photon-lib:build -x check --max-workers 2
-
-      # Run Gradle Tests.
       - name: Gradle Tests
         run: ./gradlew testHeadless -i --max-workers 1 --stacktrace
-
-      # Generate Coverage Report.
       - name: Gradle Coverage
         run: ./gradlew jacocoTestReport --max-workers 1
-
-      # Publish Coverage Report.
-      - name: Publish Server Coverage Report
+      - name: Publish Coverage Report
         uses: codecov/codecov-action@v3
         with:
           file: ./photon-server/build/reports/jacoco/test/jacocoTestReport.xml
-
       - name: Publish Core Coverage Report
         uses: codecov/codecov-action@v3
         with:
           file: ./photon-core/build/reports/jacoco/test/jacocoTestReport.xml
-  photonserver-build-offline-docs:
+  build-offline-docs:
+    name: "Build Offline Docs"
     runs-on: ubuntu-22.04
-
     steps:
-      # Checkout docs.
       - uses: actions/checkout@v3
         with:
           repository: 'PhotonVision/photonvision-docs.git'
           ref: master
-
-      # Install Python.
       - uses: actions/setup-python@v4
         with:
           python-version: '3.9'
-
       - name: Install dependencies
         run: |
           python -m pip install --upgrade pip
           pip install sphinx sphinx_rtd_theme sphinx-tabs sphinxext-opengraph doc8
           pip install -r requirements.txt
-
-      # Don't check the docs. If a PR was merged to the docs repo, it ought to pass CI. No need to re-check here.
-      # - name: Check the docs
-      #   run: |
-      #     make linkcheck
-      #     make lint
-
       - name: Build the docs
         run: |
           make html
-
-      # Upload docs artifact.
       - uses: actions/upload-artifact@master
         with:
           name: built-docs
           path: build/html
-  photonserver-check-lint:
-    # The type of runner that the job will run on.
-    runs-on: ubuntu-22.04
-
-    steps:
-      # Checkout code.
-      - uses: actions/checkout@v3
-        with:
-          fetch-depth: 0
-
-      # Install Java 17.
-      - uses: actions/setup-java@v3
-        with:
-          java-version: 17
-          distribution: temurin
-
-      # Check server code with Spotless.
-      - run: |
-          chmod +x gradlew
-          ./gradlew spotlessCheck
-  photonlib-build-host:
+  build-photonlib-host:
     env:
       MACOSX_DEPLOYMENT_TARGET: 10.14
     strategy:
@@ -232,13 +133,14 @@ jobs:
           - os: ubuntu-22.04
             artifact-name: Linux
 
-    runs-on: ${{ matrix.os }}
     name: "Photonlib - Build Host - ${{ matrix.artifact-name }}"
+    runs-on: ${{ matrix.os }}
     steps:
       - uses: actions/checkout@v3
         with:
           fetch-depth: 0
-      - uses: actions/setup-java@v3
+      - name: Install Java 17
+        uses: actions/setup-java@v3
         with:
           java-version: 17
           distribution: temurin
@@ -251,7 +153,7 @@ jobs:
         env:
           ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }}
         if: github.event_name == 'push'
-  photonlib-build-docker:
+  build-photonlib-docker:
     strategy:
       fail-fast: false
       matrix:
@@ -284,85 +186,50 @@ jobs:
         env:
           ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }}
         if: github.event_name == 'push'
-  photonlib-wpiformat:
-    name: "wpiformat"
-    runs-on: ubuntu-22.04
-    steps:
-      - 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@v4
-        with:
-          python-version: 3.8
-      - name: Install wpiformat
-        run: pip3 install wpiformat
-      - name: Check index.html not changed
-        run: git --no-pager diff --exit-code origin/master photon-server/src/main/resources/web/index.html
-      - name: Run
-        run: wpiformat
-      - name: Check output
-        run: git --no-pager diff --exit-code HEAD
-      - name: Generate diff
-        run: git diff HEAD > wpiformat-fixes.patch
-        if: ${{ failure() }}
-      - uses: actions/upload-artifact@v3
-        with:
-          name: wpiformat fixes
-          path: wpiformat-fixes.patch
-        if: ${{ failure() }}
-  photon-build-package:
-    needs: [photonclient-build, photon-build-all, photonserver-build-offline-docs]
+  build-package:
+    needs: [build-client, build-gradle, build-offline-docs]
 
     strategy:
-        fail-fast: false
-        matrix:
-          include:
-            - os: windows-latest
-              artifact-name: Win64
-              architecture: x64
-              arch-override: none
-            - os: macos-latest
-              artifact-name: macOS
-              architecture: x64
-              arch-override: none
-            - os: ubuntu-latest
-              artifact-name: Linux
-              architecture: x64
-              arch-override: none
-            - os: macos-latest
-              artifact-name: macOSArm
-              architecture: x64
-              arch-override: macarm64
-            - os: ubuntu-latest
-              artifact-name: LinuxArm32
-              architecture: x64
-              arch-override: linuxarm32
-            - os: ubuntu-latest
-              artifact-name: LinuxArm64
-              architecture: x64
-              arch-override: linuxarm64
+      fail-fast: false
+      matrix:
+        include:
+          - os: windows-latest
+            artifact-name: Win64
+            architecture: x64
+            arch-override: none
+          - os: macos-latest
+            artifact-name: macOS
+            architecture: x64
+            arch-override: none
+          - os: ubuntu-latest
+            artifact-name: Linux
+            architecture: x64
+            arch-override: none
+          - os: macos-latest
+            artifact-name: macOSArm
+            architecture: x64
+            arch-override: macarm64
+          - os: ubuntu-latest
+            artifact-name: LinuxArm32
+            architecture: x64
+            arch-override: linuxarm32
+          - os: ubuntu-latest
+            artifact-name: LinuxArm64
+            architecture: x64
+            arch-override: linuxarm64
 
-    # The type of runner that the job will run on.
     runs-on: ${{ matrix.os }}
     name: "Build fat JAR - ${{ matrix.artifact-name }}"
 
     steps:
-      # Checkout code.
       - uses: actions/checkout@v3
         with:
           fetch-depth: 0
-
-      # Install Java 17.
-      - uses: actions/setup-java@v3
+      - name: Install Java 17
+        uses: actions/setup-java@v3
         with:
           java-version: 17
           distribution: temurin
-
-      # Clear any existing web resources.
       - run: |
           rm -rf photon-server/src/main/resources/web/*
           mkdir -p photon-server/src/main/resources/web/docs
@@ -371,20 +238,14 @@ jobs:
           del photon-server\src\main\resources\web\*.*
           mkdir photon-server\src\main\resources\web\docs
         if: ${{ (matrix.os) == 'windows-latest' }}
-
-      # Download client artifact to resources folder.
       - 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@v3
         with:
           name: built-docs
           path: photon-server/src/main/resources/web/docs
-
-      # Build fat jar for both pi and everything
       - run: |
           chmod +x gradlew
           ./gradlew photon-server:shadowJar --max-workers 2 -PArchOverride=${{ matrix.arch-override }}
@@ -393,62 +254,55 @@ jobs:
           chmod +x gradlew
           ./gradlew photon-server:shadowJar --max-workers 2
         if: ${{ (matrix.arch-override == 'none') }}
-
-      # Upload final fat jar as artifact.
       - uses: actions/upload-artifact@v3
         with:
           name: jar-${{ matrix.artifact-name }}
           path: photon-server/build/libs
-  photon-image-generator:
-    needs: [photon-build-package]
+  build-image:
+    needs: [build-package]
+
     if: ${{ github.event_name != 'pull_request' }}
     strategy:
-        fail-fast: false
-        matrix:
-          include:
-            - os: ubuntu-latest
-              artifact-name: LinuxArm64
-              image_suffix: RaspberryPi
-              image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.1.3_arm64
-            - os: ubuntu-latest
-              artifact-name: LinuxArm64
-              image_suffix: limelight2
-              image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.2.2_limelight-arm64
+      fail-fast: false
+      matrix:
+        include:
+          - os: ubuntu-latest
+            artifact-name: LinuxArm64
+            image_suffix: RaspberryPi
+            image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.1.3_arm64
+          - os: ubuntu-latest
+            artifact-name: LinuxArm64
+            image_suffix: limelight2
+            image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.2.2_limelight-arm64
 
     runs-on: ${{ matrix.os }}
     name: "Build image - ${{ matrix.image_url }}"
 
     steps:
-      # Checkout code.
       - name: Checkout code
         uses: actions/checkout@v3
         with:
           fetch-depth: 0
-
       - uses: actions/download-artifact@v2
         with:
           name: jar-${{ matrix.artifact-name }}
-
       - name: Generate image
         run: |
           chmod +x scripts/generatePiImage.sh
           ./scripts/generatePiImage.sh ${{ matrix.image_url }} ${{ matrix.image_suffix }}
-
       - uses: actions/upload-artifact@v3
         name: Upload image
         with:
           name: image-${{ matrix.image_suffix }}
           path: photonvision*.xz
-  photon-release:
-    needs: [photon-build-package, photon-image-generator]
+  release:
+    needs: [build-package, build-image]
     runs-on: ubuntu-22.04
     steps:
       # Download literally every single artifact. This also downloads client and docs,
       # but the filtering below won't pick these up (I hope)
       - uses: actions/download-artifact@v2
-
       - run: find
-
       # Push to dev release
       - uses: pyTooling/Actions/releaser@r0
         with:
@@ -459,7 +313,6 @@ jobs:
             **/*.xz
             **/*.jar
         if: github.event_name == 'push'
-
       # Upload all jars and xz archives
       - uses: softprops/action-gh-release@v1
         with:
diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml
new file mode 100644
index 0000000000..0fd220a02e
--- /dev/null
+++ b/.github/workflows/lint-format.yml
@@ -0,0 +1,88 @@
+name: Lint and Format
+
+on:
+  push:
+    branches: [ master ]
+    tags:
+      - 'v*'
+  pull_request:
+    branches: [ master ]
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  wpiformat:
+    name: "wpiformat"
+    runs-on: ubuntu-22.04
+    steps:
+      - 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@v4
+        with:
+          python-version: 3.8
+      - name: Install wpiformat
+        run: pip3 install wpiformat
+      - name: Run
+        run: wpiformat
+      - name: Check output
+        run: git --no-pager diff --exit-code HEAD
+      - name: Generate diff
+        run: git diff HEAD > wpiformat-fixes.patch
+        if: ${{ failure() }}
+      - uses: actions/upload-artifact@v3
+        with:
+          name: wpiformat fixes
+          path: wpiformat-fixes.patch
+        if: ${{ failure() }}
+  javaformat:
+    name: "Java Formatting"
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+      - uses: actions/setup-java@v3
+        with:
+          java-version: 17
+          distribution: temurin
+      - run: |
+          chmod +x gradlew
+          ./gradlew spotlessCheck
+
+  client-lint-format:
+    name: "PhotonClient Lint and Formatting"
+    defaults:
+      run:
+        working-directory: photon-client
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v3
+      - name: Setup Node.js
+        uses: actions/setup-node@v3
+        with:
+          node-version: 18
+      - name: Install Dependencies
+        run: npm ci
+      - name: Check Linting
+        run: npm run lint-ci
+      - name: Check Formatting
+        run: npm run format-ci
+  server-index:
+    name: "Check server index.html not changed"
+    runs-on: ubuntu-22.04
+    steps:
+      - 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: Check index.html not changed
+        run: git --no-pager diff --exit-code origin/master photon-server/src/main/resources/web/index.html
diff --git a/.gitignore b/.gitignore
index 2bb8de0059..609770cdf5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -114,7 +114,6 @@ fabric.properties
 **/.settings
 **/.classpath
 **/.project
-**/settings
 **/dependency-reduced-pom.xml
 # photon-server/photon-vision.iml
 
diff --git a/photon-client/src/App.vue b/photon-client/src/App.vue
index bcd3e27e94..80711960f1 100644
--- a/photon-client/src/App.vue
+++ b/photon-client/src/App.vue
@@ -30,7 +30,7 @@ const websocket = new AutoReconnectingWebsocket(
       useSettingsStore().updateMetricsFromWebsocket(data.metrics);
     }
     if (data.updatePipelineResult !== undefined) {
-      useStateStore().updatePipelineResultsFromWebsocket(data.updatePipelineResult);
+      useStateStore().updateBackendResultsFromWebsocket(data.updatePipelineResult);
     }
     if (data.mutatePipelineSettings !== undefined && data.cameraIndex !== undefined) {
       useCameraSettingsStore().changePipelineSettingsInStore(data.mutatePipelineSettings, data.cameraIndex);
diff --git a/photon-client/src/components/app/photon-camera-stream.vue b/photon-client/src/components/app/photon-camera-stream.vue
index 2868ae7180..13f3e75776 100644
--- a/photon-client/src/components/app/photon-camera-stream.vue
+++ b/photon-client/src/components/app/photon-camera-stream.vue
@@ -4,13 +4,14 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { useStateStore } from "@/stores/StateStore";
 import loadingImage from "@/assets/images/loading.svg";
 import type { StyleValue } from "vue/types/jsx";
+import PvIcon from "@/components/common/pv-icon.vue";
 
 const props = defineProps<{
   streamType: "Raw" | "Processed";
   id?: string;
 }>();
 
-const src = computed<string>(() => {
+const streamSrc = computed<string>(() => {
   const port =
     useCameraSettingsStore().currentCameraSettings.stream[props.streamType === "Raw" ? "inputPort" : "outputPort"];
 
@@ -20,25 +21,74 @@ const src = computed<string>(() => {
 
   return `http://${inject("backendHostname")}:${port}/stream.mjpg`;
 });
-const alt = computed<string>(() => `${props.streamType} Stream View`);
-
-const style = computed<StyleValue>(() => {
+const streamDesc = computed<string>(() => `${props.streamType} Stream View`);
+const streamStyle = computed<StyleValue>(() => {
   if (useStateStore().colorPickingMode) {
-    return { cursor: "crosshair" };
-  } else if (src.value !== loadingImage) {
-    return { cursor: "pointer" };
+    return { width: "100%", cursor: "crosshair" };
+  } else if (streamSrc.value !== loadingImage) {
+    return { width: "100%", cursor: "pointer" };
   }
 
-  return {};
+  return { width: "100%" };
 });
 
-const handleClick = () => {
-  if (!useStateStore().colorPickingMode && src.value !== loadingImage) {
-    window.open(src.value);
+const overlayStyle = computed<StyleValue>(() => {
+  if (useStateStore().colorPickingMode || streamSrc.value == loadingImage) {
+    return { display: "none" };
+  } else {
+    return {};
+  }
+});
+
+const handleStreamClick = () => {
+  if (!useStateStore().colorPickingMode && streamSrc.value !== loadingImage) {
+    window.open(streamSrc.value);
+  }
+};
+const handleCaptureClick = () => {
+  if (props.streamType === "Raw") {
+    useCameraSettingsStore().saveInputSnapshot();
+  } else {
+    useCameraSettingsStore().saveOutputSnapshot();
   }
 };
 </script>
 
 <template>
-  <img :id="id" crossorigin="anonymous" :src="src" :alt="alt" :style="style" @click="handleClick" />
+  <div class="stream-container">
+    <img
+      :id="id"
+      crossorigin="anonymous"
+      :src="streamSrc"
+      :alt="streamDesc"
+      :style="streamStyle"
+      @click="handleStreamClick"
+    />
+    <div class="stream-overlay" :style="overlayStyle">
+      <pv-icon
+        icon-name="mdi-camera-image"
+        tooltip="Capture and save a frame of this stream"
+        class="ma-1 mr-2"
+        @click="handleCaptureClick"
+      />
+    </div>
+  </div>
 </template>
+
+<style scoped>
+.stream-container {
+  position: relative;
+}
+
+.stream-overlay {
+  opacity: 0;
+  transition: 0.1s ease;
+  position: absolute;
+  top: 0;
+  right: 0;
+}
+
+.stream-container:hover .stream-overlay {
+  opacity: 1;
+}
+</style>
diff --git a/photon-client/src/components/app/photon-log-view.vue b/photon-client/src/components/app/photon-log-view.vue
index e5442bc28c..103ea94733 100644
--- a/photon-client/src/components/app/photon-log-view.vue
+++ b/photon-client/src/components/app/photon-log-view.vue
@@ -71,7 +71,7 @@ document.addEventListener("keydown", (e) => {
           </v-btn>
         </v-btn-toggle>
         <v-card-text v-if="logs.length === 0" style="font-size: 18px; font-weight: 600">
-          There are no Logs to show
+          There are no logs to show
         </v-card-text>
         <v-virtual-scroll v-else :items="logs" item-height="50" height="600">
           <template #default="{ item }">
diff --git a/photon-client/src/components/cameras/CameraCalibrationCard.vue b/photon-client/src/components/cameras/CameraCalibrationCard.vue
index fb12224656..f9bdbf1332 100644
--- a/photon-client/src/components/cameras/CameraCalibrationCard.vue
+++ b/photon-client/src/components/cameras/CameraCalibrationCard.vue
@@ -5,11 +5,11 @@ import { CalibrationBoardTypes, type Resolution, type VideoFormat } from "@/type
 import JsPDF from "jspdf";
 import { font as PromptRegular } from "@/assets/fonts/PromptRegular";
 import MonoLogo from "@/assets/images/logoMono.png";
-import CvSlider from "@/components/common/cv-slider.vue";
+import PvSlider from "@/components/common/pv-slider.vue";
 import { useStateStore } from "@/stores/StateStore";
-import CvSwitch from "@/components/common/cv-switch.vue";
-import CvSelect from "@/components/common/cv-select.vue";
-import CvNumberInput from "@/components/common/cv-number-input.vue";
+import PvSwitch from "@/components/common/pv-switch.vue";
+import PvSelect from "@/components/common/pv-select.vue";
+import PvNumberInput from "@/components/common/pv-number-input.vue";
 import { WebsocketPipelineType } from "@/types/WebsocketDataTypes";
 
 const settingsValid = ref(true);
@@ -224,7 +224,7 @@ const endCalibration = () => {
         <v-row>
           <v-col cols="12" md="6">
             <v-form ref="form" v-model="settingsValid">
-              <cv-select
+              <pv-select
                 v-model="useStateStore().calibrationData.videoFormatIndex"
                 label="Resolution"
                 :select-cols="7"
@@ -232,7 +232,7 @@ const endCalibration = () => {
                 tooltip="Resolution to calibrate at (you will have to calibrate every resolution you use 3D mode on)"
                 :items="getUniqueVideoResolutionStrings()"
               />
-              <cv-select
+              <pv-select
                 v-model="useCameraSettingsStore().currentPipelineSettings.streamingFrameDivisor"
                 label="Decimation"
                 tooltip="Resolution to which camera frames are downscaled for detection. Calibration still uses full-res"
@@ -242,7 +242,7 @@ const endCalibration = () => {
                   (v) => useCameraSettingsStore().changeCurrentPipelineSetting({ streamingFrameDivisor: v }, false)
                 "
               />
-              <cv-select
+              <pv-select
                 v-model="boardType"
                 label="Board Type"
                 tooltip="Calibration board pattern to use"
@@ -250,7 +250,7 @@ const endCalibration = () => {
                 :items="['Chessboard', 'Dotboard']"
                 :disabled="isCalibrating"
               />
-              <cv-number-input
+              <pv-number-input
                 v-model="squareSizeIn"
                 label="Pattern Spacing (in)"
                 tooltip="Spacing between pattern features in inches"
@@ -258,7 +258,7 @@ const endCalibration = () => {
                 :rules="[(v) => v > 0 || 'Size must be positive']"
                 :label-cols="5"
               />
-              <cv-number-input
+              <pv-number-input
                 v-model="patternWidth"
                 label="Board Width (in)"
                 tooltip="Width of the board in dots or chessboard squares"
@@ -266,7 +266,7 @@ const endCalibration = () => {
                 :rules="[(v) => v >= 4 || 'Width must be at least 4']"
                 :label-cols="5"
               />
-              <cv-number-input
+              <pv-number-input
                 v-model="patternHeight"
                 label="Board Height (in)"
                 tooltip="Height of the board in dots or chessboard squares"
@@ -317,7 +317,7 @@ const endCalibration = () => {
         </v-row>
         <v-row v-if="isCalibrating">
           <v-col cols="12" class="pt-0">
-            <cv-slider
+            <pv-slider
               v-model="useCameraSettingsStore().currentPipelineSettings.cameraExposure"
               :disabled="useCameraSettingsStore().currentCameraSettings.pipelineSettings.cameraAutoExposure"
               label="Exposure"
@@ -328,7 +328,7 @@ const endCalibration = () => {
               :step="0.1"
               @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraExposure: args }, false)"
             />
-            <cv-slider
+            <pv-slider
               v-model="useCameraSettingsStore().currentPipelineSettings.cameraBrightness"
               label="Brightness"
               :min="0"
@@ -338,7 +338,7 @@ const endCalibration = () => {
                 (args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraBrightness: args }, false)
               "
             />
-            <cv-switch
+            <pv-switch
               v-model="useCameraSettingsStore().currentPipelineSettings.cameraAutoExposure"
               class="pt-2"
               label="Auto Exposure"
@@ -348,7 +348,7 @@ const endCalibration = () => {
                 (args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraAutoExposure: args }, false)
               "
             />
-            <cv-slider
+            <pv-slider
               v-if="useCameraSettingsStore().currentPipelineSettings.cameraGain >= 0"
               v-model="useCameraSettingsStore().currentPipelineSettings.cameraGain"
               label="Camera Gain"
@@ -357,7 +357,7 @@ const endCalibration = () => {
               :max="100"
               @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraGain: args }, false)"
             />
-            <cv-slider
+            <pv-slider
               v-if="useCameraSettingsStore().currentPipelineSettings.cameraRedGain !== -1"
               v-model="useCameraSettingsStore().currentPipelineSettings.cameraRedGain"
               label="Red AWB Gain"
@@ -366,7 +366,7 @@ const endCalibration = () => {
               tooltip="Controls red automatic white balance gain, which affects how the camera captures colors in different conditions"
               @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraRedGain: args }, false)"
             />
-            <cv-slider
+            <pv-slider
               v-if="useCameraSettingsStore().currentPipelineSettings.cameraBlueGain !== -1"
               v-model="useCameraSettingsStore().currentPipelineSettings.cameraBlueGain"
               label="Blue AWB Gain"
@@ -384,7 +384,7 @@ const endCalibration = () => {
               color="secondary"
               style="width: 100%"
               :disabled="!settingsValid"
-              @click="isCalibrating ? useCameraSettingsStore().takeCalibrationSnapshot(true) : startCalibration()"
+              @click="isCalibrating ? useCameraSettingsStore().takeCalibrationSnapshot() : startCalibration()"
             >
               {{ isCalibrating ? "Take Snapshot" : "Start Calibration" }}
             </v-btn>
diff --git a/photon-client/src/components/cameras/CameraSettingsCard.vue b/photon-client/src/components/cameras/CameraSettingsCard.vue
index cc40394b77..844b6d3f66 100644
--- a/photon-client/src/components/cameras/CameraSettingsCard.vue
+++ b/photon-client/src/components/cameras/CameraSettingsCard.vue
@@ -1,6 +1,6 @@
 <script setup lang="ts">
-import CvSelect from "@/components/common/cv-select.vue";
-import CvNumberInput from "@/components/common/cv-number-input.vue";
+import PvSelect from "@/components/common/pv-select.vue";
+import PvNumberInput from "@/components/common/pv-number-input.vue";
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { useStateStore } from "@/stores/StateStore";
 import { ref, watchEffect } from "vue";
@@ -47,7 +47,7 @@ watchEffect(() => {
   <v-card class="mb-3 pr-6 pb-3" color="primary" dark>
     <v-card-title>Camera Settings</v-card-title>
     <div class="ml-5">
-      <cv-select
+      <pv-select
         v-model="useStateStore().currentCameraIndex"
         label="Camera"
         :items="useCameraSettingsStore().cameraNames"
@@ -59,7 +59,7 @@ watchEffect(() => {
           }
         "
       />
-      <cv-number-input
+      <pv-number-input
         v-model="currentFov"
         :tooltip="
           !useCameraSettingsStore().currentCameraSettings.fov.managedByVendor
diff --git a/photon-client/src/components/cameras/CamerasView.vue b/photon-client/src/components/cameras/CamerasView.vue
index 8b7f1785dc..4448dcde1e 100644
--- a/photon-client/src/components/cameras/CamerasView.vue
+++ b/photon-client/src/components/cameras/CamerasView.vue
@@ -30,7 +30,7 @@ const driverMode = computed<boolean>({
 });
 
 const fpsTooLow = computed<boolean>(() => {
-  const currFPS = useStateStore().pipelineResults?.fps || 0;
+  const currFPS = useStateStore().currentPipelineResults?.fps || 0;
   const targetFPS = useCameraSettingsStore().currentVideoFormat.fps;
   const driverMode = useCameraSettingsStore().isDriverMode;
   const gpuAccel = useSettingsStore().general.gpuAcceleration !== undefined;
@@ -41,7 +41,7 @@ const fpsTooLow = computed<boolean>(() => {
 </script>
 
 <template>
-  <v-card class="mb-3 pr-6 pb-3 pa-4" color="primary" dark>
+  <v-card class="mb-3 pb-3 pa-4" color="primary" dark>
     <v-card-title
       class="pb-0 mb-2 pl-4 pt-1"
       style="min-height: 50px; justify-content: space-between; align-content: center"
@@ -58,8 +58,8 @@ const fpsTooLow = computed<boolean>(() => {
             style="font-size: 1rem; padding: 0; margin: 0"
           >
             <span class="pr-1">
-              {{ Math.round(useStateStore().pipelineResults?.fps || 0) }}&nbsp;FPS &ndash;
-              {{ Math.min(Math.round(useStateStore().pipelineResults?.latency || 0), 9999) }} ms latency
+              {{ Math.round(useStateStore().currentPipelineResults?.fps || 0) }}&nbsp;FPS &ndash;
+              {{ Math.min(Math.round(useStateStore().currentPipelineResults?.latency || 0), 9999) }} ms latency
             </span>
           </v-chip>
         </div>
@@ -133,16 +133,16 @@ th {
 .stream {
   display: flex;
   justify-content: center;
-  width: 100%;
 }
 
 @media only screen and (min-width: 512px) and (max-width: 960px) {
   .stream-container {
     flex-wrap: nowrap;
+    justify-content: center;
   }
 
   .stream {
-    width: 50%;
+    max-width: 50%;
   }
 }
 </style>
diff --git a/photon-client/src/components/common/cv-icon.vue b/photon-client/src/components/common/pv-icon.vue
similarity index 100%
rename from photon-client/src/components/common/cv-icon.vue
rename to photon-client/src/components/common/pv-icon.vue
diff --git a/photon-client/src/components/common/cv-input.vue b/photon-client/src/components/common/pv-input.vue
similarity index 96%
rename from photon-client/src/components/common/cv-input.vue
rename to photon-client/src/components/common/pv-input.vue
index a61e2ca148..ff3434132c 100644
--- a/photon-client/src/components/common/cv-input.vue
+++ b/photon-client/src/components/common/pv-input.vue
@@ -1,6 +1,6 @@
 <script setup lang="ts">
 import { computed } from "vue";
-import TooltippedLabel from "@/components/common/cv-tooltipped-label.vue";
+import TooltippedLabel from "@/components/common/pv-tooltipped-label.vue";
 
 const props = withDefaults(
   defineProps<{
diff --git a/photon-client/src/components/common/cv-number-input.vue b/photon-client/src/components/common/pv-number-input.vue
similarity index 94%
rename from photon-client/src/components/common/cv-number-input.vue
rename to photon-client/src/components/common/pv-number-input.vue
index 67237bbb7f..8b359a7540 100644
--- a/photon-client/src/components/common/cv-number-input.vue
+++ b/photon-client/src/components/common/pv-number-input.vue
@@ -1,5 +1,5 @@
 <script setup lang="ts">
-import TooltippedLabel from "@/components/common/cv-tooltipped-label.vue";
+import TooltippedLabel from "@/components/common/pv-tooltipped-label.vue";
 import { computed } from "vue";
 
 const props = withDefaults(
diff --git a/photon-client/src/components/common/cv-radio.vue b/photon-client/src/components/common/pv-radio.vue
similarity index 94%
rename from photon-client/src/components/common/cv-radio.vue
rename to photon-client/src/components/common/pv-radio.vue
index 33006fe4c9..8777f719be 100644
--- a/photon-client/src/components/common/cv-radio.vue
+++ b/photon-client/src/components/common/pv-radio.vue
@@ -1,6 +1,6 @@
 <script setup lang="ts">
 import { computed } from "vue";
-import TooltippedLabel from "@/components/common/cv-tooltipped-label.vue";
+import TooltippedLabel from "@/components/common/pv-tooltipped-label.vue";
 
 const props = withDefaults(
   defineProps<{
diff --git a/photon-client/src/components/common/cv-range-slider.vue b/photon-client/src/components/common/pv-range-slider.vue
similarity index 97%
rename from photon-client/src/components/common/cv-range-slider.vue
rename to photon-client/src/components/common/pv-range-slider.vue
index 9367993264..fb23ac9479 100644
--- a/photon-client/src/components/common/cv-range-slider.vue
+++ b/photon-client/src/components/common/pv-range-slider.vue
@@ -1,6 +1,6 @@
 <script setup lang="ts">
 import { computed } from "vue";
-import TooltippedLabel from "@/components/common/cv-tooltipped-label.vue";
+import TooltippedLabel from "@/components/common/pv-tooltipped-label.vue";
 
 const props = withDefaults(
   defineProps<{
diff --git a/photon-client/src/components/common/cv-select.vue b/photon-client/src/components/common/pv-select.vue
similarity index 96%
rename from photon-client/src/components/common/cv-select.vue
rename to photon-client/src/components/common/pv-select.vue
index cc5ee815e3..a681914d45 100644
--- a/photon-client/src/components/common/cv-select.vue
+++ b/photon-client/src/components/common/pv-select.vue
@@ -1,6 +1,6 @@
 <script setup lang="ts">
 import { computed } from "vue";
-import TooltippedLabel from "@/components/common/cv-tooltipped-label.vue";
+import TooltippedLabel from "@/components/common/pv-tooltipped-label.vue";
 
 interface SelectItem {
   name: string | number;
diff --git a/photon-client/src/components/common/cv-slider.vue b/photon-client/src/components/common/pv-slider.vue
similarity index 96%
rename from photon-client/src/components/common/cv-slider.vue
rename to photon-client/src/components/common/pv-slider.vue
index 74138793ca..0fb59d61a1 100644
--- a/photon-client/src/components/common/cv-slider.vue
+++ b/photon-client/src/components/common/pv-slider.vue
@@ -1,6 +1,6 @@
 <script setup lang="ts">
 import { computed } from "vue";
-import TooltippedLabel from "@/components/common/cv-tooltipped-label.vue";
+import TooltippedLabel from "@/components/common/pv-tooltipped-label.vue";
 
 const props = withDefaults(
   defineProps<{
diff --git a/photon-client/src/components/common/cv-switch.vue b/photon-client/src/components/common/pv-switch.vue
similarity index 93%
rename from photon-client/src/components/common/cv-switch.vue
rename to photon-client/src/components/common/pv-switch.vue
index 58cd3d6ff8..d3115807d8 100644
--- a/photon-client/src/components/common/cv-switch.vue
+++ b/photon-client/src/components/common/pv-switch.vue
@@ -1,5 +1,5 @@
 <script setup lang="ts">
-import TooltippedLabel from "@/components/common/cv-tooltipped-label.vue";
+import TooltippedLabel from "@/components/common/pv-tooltipped-label.vue";
 import { computed } from "vue";
 
 const props = withDefaults(
diff --git a/photon-client/src/components/common/cv-tooltipped-label.vue b/photon-client/src/components/common/pv-tooltipped-label.vue
similarity index 100%
rename from photon-client/src/components/common/cv-tooltipped-label.vue
rename to photon-client/src/components/common/pv-tooltipped-label.vue
diff --git a/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue b/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue
index bf3a7a5df1..aca5bc4975 100644
--- a/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue
+++ b/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue
@@ -1,11 +1,11 @@
 <script setup lang="ts">
-import CvSelect from "@/components/common/cv-select.vue";
+import PvSelect from "@/components/common/pv-select.vue";
 import { useStateStore } from "@/stores/StateStore";
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { WebsocketPipelineType } from "@/types/WebsocketDataTypes";
 import { computed, ref } from "vue";
-import CvIcon from "@/components/common/cv-icon.vue";
-import CvInput from "@/components/common/cv-input.vue";
+import PvIcon from "@/components/common/pv-icon.vue";
+import PvInput from "@/components/common/pv-input.vue";
 import { PipelineType } from "@/types/PipelineTypes";
 
 const changeCurrentCameraIndex = (index: number) => {
@@ -211,14 +211,14 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
   <v-card color="primary">
     <v-row style="padding: 12px 12px 0 24px">
       <v-col cols="10" class="pa-0">
-        <cv-select
+        <pv-select
           v-if="!isCameraNameEdit"
           v-model="useStateStore().currentCameraIndex"
           label="Camera"
           :items="useCameraSettingsStore().cameraNames"
           @input="changeCurrentCameraIndex"
         />
-        <cv-input
+        <pv-input
           v-else
           v-model="currentCameraName"
           class="pt-2"
@@ -230,12 +230,12 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
         />
       </v-col>
       <v-col cols="2" style="display: flex; align-items: center; justify-content: center">
-        <cv-icon color="#c5c5c5" icon-name="mdi-pencil" tooltip="Edit Camera Name" @click="startCameraNameEdit" />
+        <pv-icon color="#c5c5c5" icon-name="mdi-pencil" tooltip="Edit Camera Name" @click="startCameraNameEdit" />
       </v-col>
     </v-row>
     <v-row style="padding: 0 12px 0 24px">
       <v-col cols="10" class="pa-0">
-        <cv-select
+        <pv-select
           v-if="!isPipelineNameEdit"
           :value="useCameraSettingsStore().currentCameraSettings.currentPipelineIndex"
           label="Pipeline"
@@ -244,7 +244,7 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
           :items="pipelineNamesWrapper"
           @input="(args) => useCameraSettingsStore().changeCurrentPipelineIndex(args, true)"
         />
-        <cv-input
+        <pv-input
           v-else
           v-model="currentPipelineName"
           :input-cols="12 - 3"
@@ -262,17 +262,17 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
           <v-list dark dense color="primary">
             <v-list-item @click="startPipelineNameEdit">
               <v-list-item-title>
-                <cv-icon color="#c5c5c5" :right="true" icon-name="mdi-pencil" tooltip="Edit pipeline name" />
+                <pv-icon color="#c5c5c5" :right="true" icon-name="mdi-pencil" tooltip="Edit pipeline name" />
               </v-list-item-title>
             </v-list-item>
             <v-list-item @click="showCreatePipelineDialog">
               <v-list-item-title>
-                <cv-icon color="#c5c5c5" :right="true" icon-name="mdi-plus" tooltip="Add new pipeline" />
+                <pv-icon color="#c5c5c5" :right="true" icon-name="mdi-plus" tooltip="Add new pipeline" />
               </v-list-item-title>
             </v-list-item>
             <v-list-item @click="showPipelineDeletionConfirmationDialog = true">
               <v-list-item-title>
-                <cv-icon color="red darken-2" :right="true" icon-name="mdi-delete" tooltip="Delete pipeline" />
+                <pv-icon color="red darken-2" :right="true" icon-name="mdi-delete" tooltip="Delete pipeline" />
               </v-list-item-title>
             </v-list-item>
             <v-list-item
@@ -283,7 +283,7 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
               "
             >
               <v-list-item-title>
-                <cv-icon color="#c5c5c5" :right="true" icon-name="mdi-content-copy" tooltip="Duplicate pipeline" />
+                <pv-icon color="#c5c5c5" :right="true" icon-name="mdi-content-copy" tooltip="Duplicate pipeline" />
               </v-list-item-title>
             </v-list-item>
           </v-list>
@@ -292,7 +292,7 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
     </v-row>
     <v-row style="padding: 0 12px 12px 24px">
       <v-col cols="10" class="pa-0">
-        <cv-select
+        <pv-select
           v-model="currentPipelineType"
           label="Type"
           tooltip="Changes the pipeline type, which changes the type of processing that will happen on input frames"
@@ -306,7 +306,7 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
       <v-card dark color="primary">
         <v-card-title> Create New Pipeline </v-card-title>
         <v-card-text>
-          <cv-input
+          <pv-input
             v-model="newPipelineName"
             placeholder="Pipeline Name"
             :label-cols="3"
@@ -314,7 +314,7 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
             label="Pipeline Name"
             :rules="[(v) => checkPipelineName(v)]"
           />
-          <cv-select
+          <pv-select
             v-model="newPipelineType"
             :select-cols="12 - 3"
             label="Tracking Type"
diff --git a/photon-client/src/components/dashboard/CamerasCard.vue b/photon-client/src/components/dashboard/CamerasCard.vue
index 8500f13c42..5670106df3 100644
--- a/photon-client/src/components/dashboard/CamerasCard.vue
+++ b/photon-client/src/components/dashboard/CamerasCard.vue
@@ -21,7 +21,7 @@ const driverMode = computed<boolean>({
 });
 
 const fpsTooLow = computed<boolean>(() => {
-  const currFPS = useStateStore().pipelineResults?.fps || 0;
+  const currFPS = useStateStore().currentPipelineResults?.fps || 0;
   const targetFPS = useCameraSettingsStore().currentVideoFormat.fps;
   const driverMode = useCameraSettingsStore().isDriverMode;
   const gpuAccel = useSettingsStore().general.gpuAcceleration !== undefined;
@@ -46,7 +46,7 @@ const fpsTooLow = computed<boolean>(() => {
           style="font-size: 1rem; padding: 0; margin: 0"
         >
           <span class="pr-1">
-            Processing @ {{ Math.round(useStateStore().pipelineResults?.fps || 0) }}&nbsp;FPS &ndash;
+            Processing @ {{ Math.round(useStateStore().currentPipelineResults?.fps || 0) }}&nbsp;FPS &ndash;
           </span>
           <span
             v-if="
@@ -61,7 +61,7 @@ const fpsTooLow = computed<boolean>(() => {
             stop viewing the raw stream for better performance
           </span>
           <span v-else>
-            {{ Math.min(Math.round(useStateStore().pipelineResults?.latency || 0), 9999) }} ms latency
+            {{ Math.min(Math.round(useStateStore().currentPipelineResults?.latency || 0), 9999) }} ms latency
           </span>
         </v-chip>
       </div>
diff --git a/photon-client/src/components/dashboard/ConfigOptions.vue b/photon-client/src/components/dashboard/ConfigOptions.vue
index 33a17ebe14..6ed0156f9f 100644
--- a/photon-client/src/components/dashboard/ConfigOptions.vue
+++ b/photon-client/src/components/dashboard/ConfigOptions.vue
@@ -104,17 +104,19 @@ const tabGroups = computed<ConfigOption[][]>(() => {
   const isAprilTag = useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.AprilTag;
   const isAruco = useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.Aruco;
 
-  return getTabGroups().map((tabGroup) =>
-    tabGroup.filter(
-      (tabConfig) =>
-        !(!allow3d && tabConfig.tabName === "3D") && //Filter out 3D tab any time 3D isn't calibrated
-        !((!allow3d || isAprilTag || isAruco) && tabConfig.tabName === "PnP") && //Filter out the PnP config tab if 3D isn't available, or we're doing AprilTags
-        !((isAprilTag || isAruco) && tabConfig.tabName === "Threshold") && //Filter out threshold tab if we're doing AprilTags
-        !((isAprilTag || isAruco) && tabConfig.tabName === "Contours") && //Filter out contours if we're doing AprilTags
-        !(!isAprilTag && tabConfig.tabName === "AprilTag") && //Filter out apriltag unless we actually are doing AprilTags
-        !(!isAruco && tabConfig.tabName === "Aruco") //Filter out aruco unless we actually are doing Aruco
+  return getTabGroups()
+    .map((tabGroup) =>
+      tabGroup.filter(
+        (tabConfig) =>
+          !(!allow3d && tabConfig.tabName === "3D") && //Filter out 3D tab any time 3D isn't calibrated
+          !((!allow3d || isAprilTag || isAruco) && tabConfig.tabName === "PnP") && //Filter out the PnP config tab if 3D isn't available, or we're doing AprilTags
+          !((isAprilTag || isAruco) && tabConfig.tabName === "Threshold") && //Filter out threshold tab if we're doing AprilTags
+          !((isAprilTag || isAruco) && tabConfig.tabName === "Contours") && //Filter out contours if we're doing AprilTags
+          !(!isAprilTag && tabConfig.tabName === "AprilTag") && //Filter out apriltag unless we actually are doing AprilTags
+          !(!isAruco && tabConfig.tabName === "Aruco") //Filter out aruco unless we actually are doing Aruco
+      )
     )
-  );
+    .filter((it) => it.length); // Remove empty tab groups
 });
 
 onBeforeUpdate(() => {
diff --git a/photon-client/src/components/dashboard/tabs/AprilTagTab.vue b/photon-client/src/components/dashboard/tabs/AprilTagTab.vue
index 5da6d959ff..fa56016a56 100644
--- a/photon-client/src/components/dashboard/tabs/AprilTagTab.vue
+++ b/photon-client/src/components/dashboard/tabs/AprilTagTab.vue
@@ -1,9 +1,9 @@
 <script setup lang="ts">
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { PipelineType } from "@/types/PipelineTypes";
-import CvSelect from "@/components/common/cv-select.vue";
-import CvSlider from "@/components/common/cv-slider.vue";
-import CvSwitch from "@/components/common/cv-switch.vue";
+import PvSelect from "@/components/common/pv-select.vue";
+import PvSlider from "@/components/common/pv-slider.vue";
+import PvSwitch from "@/components/common/pv-switch.vue";
 import { computed, getCurrentInstance } from "vue";
 import { useStateStore } from "@/stores/StateStore";
 
@@ -22,14 +22,14 @@ const interactiveCols = computed(
 
 <template>
   <div v-if="currentPipelineSettings.pipelineType === PipelineType.AprilTag">
-    <cv-select
+    <pv-select
       v-model="currentPipelineSettings.tagFamily"
       label="Target family"
       :items="['AprilTag Family 36h11', 'AprilTag Family 25h9', 'AprilTag Family 16h5']"
       :select-cols="interactiveCols"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ tagFamily: value }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-model="currentPipelineSettings.decimate"
       class="pt-2"
       :slider-cols="interactiveCols"
@@ -39,7 +39,7 @@ const interactiveCols = computed(
       :max="8"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ decimate: value }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-model="currentPipelineSettings.blur"
       class="pt-2"
       :slider-cols="interactiveCols"
@@ -50,7 +50,7 @@ const interactiveCols = computed(
       :step="0.1"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ blur: value }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-model="currentPipelineSettings.threads"
       class="pt-2"
       :slider-cols="interactiveCols"
@@ -60,14 +60,14 @@ const interactiveCols = computed(
       :max="8"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ threads: value }, false)"
     />
-    <cv-switch
+    <pv-switch
       v-model="currentPipelineSettings.refineEdges"
       class="pt-2"
       label="Refine Edges"
       tooltip="Further refines the AprilTag corner position initial estimate, suggested left on"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ refineEdges: value }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-model="currentPipelineSettings.decisionMargin"
       class="pt-2 pb-4"
       :slider-cols="interactiveCols"
@@ -77,7 +77,7 @@ const interactiveCols = computed(
       :max="250"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ decisionMargin: value }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-model="currentPipelineSettings.numIterations"
       class="pt-2 pb-4"
       :slider-cols="interactiveCols"
diff --git a/photon-client/src/components/dashboard/tabs/ArucoTab.vue b/photon-client/src/components/dashboard/tabs/ArucoTab.vue
index fc53fc1bb8..469700fdef 100644
--- a/photon-client/src/components/dashboard/tabs/ArucoTab.vue
+++ b/photon-client/src/components/dashboard/tabs/ArucoTab.vue
@@ -1,9 +1,7 @@
 <script setup lang="ts">
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { PipelineType } from "@/types/PipelineTypes";
-import CvSlider from "@/components/common/cv-slider.vue";
-import CvRangeSlider from "@/components/common/cv-range-slider.vue";
-import CvSwitch from "@/components/common/cv-switch.vue";
+import PvSlider from "@/components/common/pv-slider.vue";
 import { computed, getCurrentInstance } from "vue";
 import { useStateStore } from "@/stores/StateStore";
 
@@ -38,7 +36,7 @@ const interactiveCols = computed(
 
 <template>
   <div v-if="currentPipelineSettings.pipelineType === PipelineType.Aruco">
-    <cv-switch
+    <pv-slider
       v-model="currentPipelineSettings.useCornerRefinement"
       class="pt-2"
       label="Refine Corners"
@@ -55,7 +53,7 @@ const interactiveCols = computed(
       :step="2"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ threshWinSizes: value }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-model="currentPipelineSettings.threshStepSize"
       class="pt-2"
       :slider-cols="interactiveCols"
@@ -66,7 +64,7 @@ const interactiveCols = computed(
       :step="1"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ threshStepSize: value }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-model="currentPipelineSettings.threshConstant"
       class="pt-2"
       :slider-cols="interactiveCols"
diff --git a/photon-client/src/components/dashboard/tabs/ContoursTab.vue b/photon-client/src/components/dashboard/tabs/ContoursTab.vue
index f99e679a38..dfdc65d2cf 100644
--- a/photon-client/src/components/dashboard/tabs/ContoursTab.vue
+++ b/photon-client/src/components/dashboard/tabs/ContoursTab.vue
@@ -1,9 +1,9 @@
 <script setup lang="ts">
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { PipelineType } from "@/types/PipelineTypes";
-import CvRangeSlider from "@/components/common/cv-range-slider.vue";
-import CvSelect from "@/components/common/cv-select.vue";
-import CvSlider from "@/components/common/cv-slider.vue";
+import PvRangeSlider from "@/components/common/pv-range-slider.vue";
+import PvSelect from "@/components/common/pv-select.vue";
+import PvSlider from "@/components/common/pv-slider.vue";
 import { computed, getCurrentInstance } from "vue";
 import { useStateStore } from "@/stores/StateStore";
 
@@ -11,7 +11,7 @@ import { useStateStore } from "@/stores/StateStore";
 // Defer reference to store access method
 const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings;
 
-// TODO fix cv-range-slider so that store access doesn't need to be deferred
+// TODO fix pv-range-slider so that store access doesn't need to be deferred
 const contourArea = computed<[number, number]>({
   get: () => Object.values(useCameraSettingsStore().currentPipelineSettings.contourArea) as [number, number],
   set: (v) => (useCameraSettingsStore().currentPipelineSettings.contourArea = v)
@@ -58,7 +58,7 @@ const interactiveCols = computed(
 
 <template>
   <div>
-    <cv-range-slider
+    <pv-range-slider
       v-model="contourArea"
       label="Area"
       :min="0"
@@ -67,7 +67,7 @@ const interactiveCols = computed(
       :step="0.01"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourArea: value }, false)"
     />
-    <cv-range-slider
+    <pv-range-slider
       v-if="useCameraSettingsStore().currentPipelineType !== PipelineType.ColoredShape"
       v-model="contourRatio"
       label="Ratio (W/H)"
@@ -78,7 +78,7 @@ const interactiveCols = computed(
       :step="0.1"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourRatio: value }, false)"
     />
-    <cv-select
+    <pv-select
       v-model="useCameraSettingsStore().currentPipelineSettings.contourTargetOrientation"
       label="Target Orientation"
       tooltip="Used to determine how to calculate target landmarks, as well as aspect ratio"
@@ -88,7 +88,7 @@ const interactiveCols = computed(
         (value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourTargetOrientation: value }, false)
       "
     />
-    <cv-range-slider
+    <pv-range-slider
       v-if="useCameraSettingsStore().currentPipelineType === PipelineType.ColoredShape"
       v-model="contourFullness"
       label="Fullness"
@@ -98,7 +98,7 @@ const interactiveCols = computed(
       :slider-cols="interactiveCols"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourFullness: value }, false)"
     />
-    <cv-range-slider
+    <pv-range-slider
       v-if="currentPipelineSettings.pipelineType === PipelineType.ColoredShape"
       v-model="contourPerimeter"
       label="Perimeter"
@@ -108,7 +108,7 @@ const interactiveCols = computed(
       :slider-cols="interactiveCols"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourPerimeter: value }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-model="useCameraSettingsStore().currentPipelineSettings.contourSpecklePercentage"
       label="Speckle Rejection"
       tooltip="Rejects contours whose average area is less than the given percentage of the average area of all the other contours"
@@ -120,7 +120,7 @@ const interactiveCols = computed(
       "
     />
     <template v-if="currentPipelineSettings.pipelineType === PipelineType.Reflective">
-      <cv-slider
+      <pv-slider
         v-model="currentPipelineSettings.contourFilterRangeX"
         label="X Filter Tightness"
         tooltip="Rejects contours whose center X is further than X standard deviations left/right of the mean X location"
@@ -130,7 +130,7 @@ const interactiveCols = computed(
         :slider-cols="interactiveCols"
         @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourFilterRangeX: value }, false)"
       />
-      <cv-slider
+      <pv-slider
         v-model="currentPipelineSettings.contourFilterRangeY"
         label="Y Filter Tightness"
         tooltip="Rejects contours whose center Y is further than X standard deviations above/below the mean Y location"
@@ -140,7 +140,7 @@ const interactiveCols = computed(
         :slider-cols="interactiveCols"
         @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourFilterRangeY: value }, false)"
       />
-      <cv-select
+      <pv-select
         v-model="useCameraSettingsStore().currentPipelineSettings.contourGroupingMode"
         label="Target Grouping"
         tooltip="Whether or not every two targets are paired with each other (good for e.g. 2019 targets)"
@@ -148,7 +148,7 @@ const interactiveCols = computed(
         :items="['Single', 'Dual', 'Two or More']"
         @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourGroupingMode: value }, false)"
       />
-      <cv-select
+      <pv-select
         v-model="useCameraSettingsStore().currentPipelineSettings.contourIntersection"
         label="Target Intersection"
         tooltip="If target grouping is in dual mode it will use this dropdown to decide how targets are grouped with adjacent targets"
@@ -160,7 +160,7 @@ const interactiveCols = computed(
     </template>
     <template v-else-if="currentPipelineSettings.pipelineType === PipelineType.ColoredShape">
       <v-divider class="mt-3" />
-      <cv-select
+      <pv-select
         v-model="currentPipelineSettings.contourShape"
         label="Target Shape"
         tooltip="The shape of targets to look for"
@@ -168,7 +168,7 @@ const interactiveCols = computed(
         :items="['Circle', 'Polygon', 'Triangle', 'Quadrilateral']"
         @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourShape: value }, false)"
       />
-      <cv-slider
+      <pv-slider
         v-model="currentPipelineSettings.accuracyPercentage"
         :disabled="currentPipelineSettings.contourShape < 1"
         label="Shape Simplification"
@@ -178,7 +178,7 @@ const interactiveCols = computed(
         :slider-cols="interactiveCols"
         @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ accuracyPercentage: value }, false)"
       />
-      <cv-slider
+      <pv-slider
         v-model="currentPipelineSettings.circleDetectThreshold"
         :disabled="currentPipelineSettings.contourShape !== 0"
         label="Circle match distance"
@@ -190,7 +190,7 @@ const interactiveCols = computed(
           (value) => useCameraSettingsStore().changeCurrentPipelineSetting({ circleDetectThreshold: value }, false)
         "
       />
-      <cv-range-slider
+      <pv-range-slider
         v-model="contourRadius"
         :disabled="currentPipelineSettings.contourShape !== 0"
         label="Radius"
@@ -199,7 +199,7 @@ const interactiveCols = computed(
         :slider-cols="interactiveCols"
         @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourRadius: value }, false)"
       />
-      <cv-slider
+      <pv-slider
         v-model="currentPipelineSettings.maxCannyThresh"
         :disabled="currentPipelineSettings.contourShape !== 0"
         label="Max Canny Threshold"
@@ -208,7 +208,7 @@ const interactiveCols = computed(
         :slider-cols="interactiveCols"
         @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ maxCannyThresh: value }, false)"
       />
-      <cv-slider
+      <pv-slider
         v-model="currentPipelineSettings.circleAccuracy"
         :disabled="currentPipelineSettings.contourShape !== 0"
         label="Circle Accuracy"
@@ -219,7 +219,7 @@ const interactiveCols = computed(
       />
       <v-divider class="mt-3" />
     </template>
-    <cv-select
+    <pv-select
       v-model="useCameraSettingsStore().currentPipelineSettings.contourSortMode"
       label="Target Sort"
       tooltip="Chooses the sorting mode used to determine the 'best' targets to provide to user code"
diff --git a/photon-client/src/components/dashboard/tabs/InputTab.vue b/photon-client/src/components/dashboard/tabs/InputTab.vue
index 1780736e0b..52cfef129e 100644
--- a/photon-client/src/components/dashboard/tabs/InputTab.vue
+++ b/photon-client/src/components/dashboard/tabs/InputTab.vue
@@ -1,8 +1,8 @@
 <script setup lang="ts">
-import CvSlider from "@/components/common/cv-slider.vue";
+import PvSlider from "@/components/common/pv-slider.vue";
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
-import CvSwitch from "@/components/common/cv-switch.vue";
-import CvSelect from "@/components/common/cv-select.vue";
+import PvSwitch from "@/components/common/pv-switch.vue";
+import PvSelect from "@/components/common/pv-select.vue";
 import { computed, getCurrentInstance } from "vue";
 import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
 import { useStateStore } from "@/stores/StateStore";
@@ -69,7 +69,7 @@ const interactiveCols = computed(
 
 <template>
   <div>
-    <cv-slider
+    <pv-slider
       v-model="useCameraSettingsStore().currentPipelineSettings.cameraExposure"
       :disabled="useCameraSettingsStore().currentCameraSettings.pipelineSettings.cameraAutoExposure"
       label="Exposure"
@@ -80,7 +80,7 @@ const interactiveCols = computed(
       :step="0.1"
       @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraExposure: args }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-model="useCameraSettingsStore().currentPipelineSettings.cameraBrightness"
       label="Brightness"
       :min="0"
@@ -88,7 +88,7 @@ const interactiveCols = computed(
       :slider-cols="interactiveCols"
       @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraBrightness: args }, false)"
     />
-    <cv-switch
+    <pv-switch
       v-model="useCameraSettingsStore().currentPipelineSettings.cameraAutoExposure"
       class="pt-2"
       label="Auto Exposure"
@@ -96,7 +96,7 @@ const interactiveCols = computed(
       tooltip="Enables or Disables camera automatic adjustment for current lighting conditions"
       @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraAutoExposure: args }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-if="useCameraSettingsStore().currentPipelineSettings.cameraGain >= 0"
       v-model="useCameraSettingsStore().currentPipelineSettings.cameraGain"
       label="Camera Gain"
@@ -106,7 +106,7 @@ const interactiveCols = computed(
       :slider-cols="interactiveCols"
       @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraGain: args }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-if="useCameraSettingsStore().currentPipelineSettings.cameraRedGain !== -1"
       v-model="useCameraSettingsStore().currentPipelineSettings.cameraRedGain"
       label="Red AWB Gain"
@@ -116,7 +116,7 @@ const interactiveCols = computed(
       tooltip="Controls red automatic white balance gain, which affects how the camera captures colors in different conditions"
       @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraRedGain: args }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-if="useCameraSettingsStore().currentPipelineSettings.cameraBlueGain !== -1"
       v-model="useCameraSettingsStore().currentPipelineSettings.cameraBlueGain"
       label="Blue AWB Gain"
@@ -126,7 +126,7 @@ const interactiveCols = computed(
       tooltip="Controls blue automatic white balance gain, which affects how the camera captures colors in different conditions"
       @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraBlueGain: args }, false)"
     />
-    <cv-select
+    <pv-select
       v-model="useCameraSettingsStore().currentPipelineSettings.inputImageRotationMode"
       label="Orientation"
       tooltip="Rotates the camera stream"
@@ -134,7 +134,7 @@ const interactiveCols = computed(
       :select-cols="interactiveCols"
       @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ inputImageRotationMode: args }, false)"
     />
-    <cv-select
+    <pv-select
       v-model="useCameraSettingsStore().currentPipelineSettings.cameraVideoModeIndex"
       label="Resolution"
       tooltip="Resolution and FPS the camera should directly capture at"
@@ -142,7 +142,7 @@ const interactiveCols = computed(
       :select-cols="interactiveCols"
       @input="(args) => handleResolutionChange(args)"
     />
-    <cv-select
+    <pv-select
       v-model="useCameraSettingsStore().currentPipelineSettings.streamingFrameDivisor"
       label="Stream Resolution"
       tooltip="Resolution to which camera frames are downscaled for streaming to the dashboard"
diff --git a/photon-client/src/components/dashboard/tabs/Map3DTab.vue b/photon-client/src/components/dashboard/tabs/Map3DTab.vue
index ac0c99801d..5ed12bfa01 100644
--- a/photon-client/src/components/dashboard/tabs/Map3DTab.vue
+++ b/photon-client/src/components/dashboard/tabs/Map3DTab.vue
@@ -4,7 +4,7 @@ import type { PhotonTarget } from "@/types/PhotonTrackingTypes";
 import { useStateStore } from "@/stores/StateStore";
 import Photon3dVisualizer from "@/components/app/photon-3d-visualizer.vue";
 
-const trackedTargets = computed<PhotonTarget[]>(() => useStateStore().pipelineResults?.targets || []);
+const trackedTargets = computed<PhotonTarget[]>(() => useStateStore().currentPipelineResults?.targets || []);
 </script>
 
 <template>
diff --git a/photon-client/src/components/dashboard/tabs/OutputTab.vue b/photon-client/src/components/dashboard/tabs/OutputTab.vue
index 06d51b6ddc..5116b6ee38 100644
--- a/photon-client/src/components/dashboard/tabs/OutputTab.vue
+++ b/photon-client/src/components/dashboard/tabs/OutputTab.vue
@@ -1,8 +1,8 @@
 <script setup lang="ts">
-import CvSelect from "@/components/common/cv-select.vue";
+import PvSelect from "@/components/common/pv-select.vue";
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { PipelineType, RobotOffsetPointMode } from "@/types/PipelineTypes";
-import CvSwitch from "@/components/common/cv-switch.vue";
+import PvSwitch from "@/components/common/pv-switch.vue";
 import { computed, getCurrentInstance } from "vue";
 import { RobotOffsetType } from "@/types/SettingTypes";
 import { useStateStore } from "@/stores/StateStore";
@@ -40,6 +40,8 @@ const offsetPoints = computed<MetricItem[]>(() => {
   }
 });
 
+const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings;
+
 const interactiveCols = computed(
   () =>
     (getCurrentInstance()?.proxy.$vuetify.breakpoint.mdAndDown || false) &&
@@ -51,7 +53,7 @@ const interactiveCols = computed(
 
 <template>
   <div>
-    <cv-select
+    <pv-select
       v-model="useCameraSettingsStore().currentPipelineSettings.contourTargetOffsetPointEdge"
       label="Target Offset Point"
       tooltip="Changes where the 'center' of the target is (used for calculating e.g. pitch and yaw)"
@@ -61,7 +63,7 @@ const interactiveCols = computed(
         (value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourTargetOffsetPointEdge: value }, false)
       "
     />
-    <cv-select
+    <pv-select
       v-if="!isTagPipeline"
       v-model="useCameraSettingsStore().currentPipelineSettings.contourTargetOrientation"
       label="Target Orientation"
@@ -72,16 +74,40 @@ const interactiveCols = computed(
         (value) => useCameraSettingsStore().changeCurrentPipelineSetting({ contourTargetOrientation: value }, false)
       "
     />
-    <cv-switch
+    <pv-switch
       v-model="useCameraSettingsStore().currentPipelineSettings.outputShowMultipleTargets"
       label="Show Multiple Targets"
-      tooltip="If enabled, up to five targets will be displayed and sent to user code, instead of just one"
+      tooltip="If enabled, up to five targets will be displayed and sent via PhotonLib, instead of just one"
       :disabled="isTagPipeline"
       :switch-cols="interactiveCols"
       @input="
         (value) => useCameraSettingsStore().changeCurrentPipelineSetting({ outputShowMultipleTargets: value }, false)
       "
     />
+    <cv-switch
+      v-if="
+        currentPipelineSettings.pipelineType === PipelineType.AprilTag &&
+        useCameraSettingsStore().isCurrentVideoFormatCalibrated
+      "
+      v-model="currentPipelineSettings.doMultiTarget"
+      label="Do Multi-Target Estimation"
+      tooltip="If enabled, all visible fiducial targets will be used to provide a single pose estimate from their combined model."
+      :switch-cols="interactiveCols"
+      :disabled="!isTagPipeline"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ doMultiTarget: value }, false)"
+    />
+    <cv-switch
+      v-if="
+        currentPipelineSettings.pipelineType === PipelineType.AprilTag &&
+        useCameraSettingsStore().isCurrentVideoFormatCalibrated
+      "
+      v-model="currentPipelineSettings.doSingleTargetAlways"
+      label="Always Do Single-Target Estimation"
+      tooltip="If disabled, visible fiducial targets used for multi-target estimation will not also be used for single-target estimation."
+      :switch-cols="interactiveCols"
+      :disabled="!isTagPipeline || !currentPipelineSettings.doMultiTarget"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ doSingleTargetAlways: value }, false)"
+    />
     <v-divider />
     <table
       v-if="useCameraSettingsStore().currentPipelineSettings.offsetRobotOffsetMode !== RobotOffsetPointMode.None"
@@ -98,7 +124,7 @@ const interactiveCols = computed(
         </td>
       </tr>
     </table>
-    <cv-select
+    <pv-select
       v-model="useCameraSettingsStore().currentPipelineSettings.offsetRobotOffsetMode"
       label="Robot Offset Mode"
       tooltip="Used to add an arbitrary offset to the location of the targeting crosshair"
diff --git a/photon-client/src/components/dashboard/tabs/PnPTab.vue b/photon-client/src/components/dashboard/tabs/PnPTab.vue
index e129759b8f..dddf4b072a 100644
--- a/photon-client/src/components/dashboard/tabs/PnPTab.vue
+++ b/photon-client/src/components/dashboard/tabs/PnPTab.vue
@@ -1,8 +1,8 @@
 <script setup lang="ts">
-import CvSelect from "@/components/common/cv-select.vue";
+import PvSelect from "@/components/common/pv-select.vue";
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { TargetModel } from "@/types/PipelineTypes";
-import CvSlider from "@/components/common/cv-slider.vue";
+import PvSlider from "@/components/common/pv-slider.vue";
 import { computed, getCurrentInstance } from "vue";
 import { useStateStore } from "@/stores/StateStore";
 
@@ -17,7 +17,7 @@ const interactiveCols = computed(
 
 <template>
   <div>
-    <cv-select
+    <pv-select
       v-model="useCameraSettingsStore().currentPipelineSettings.targetModel"
       label="Target Model"
       :items="[
@@ -34,7 +34,7 @@ const interactiveCols = computed(
       :select-cols="interactiveCols"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ targetModel: value }, false)"
     />
-    <cv-slider
+    <pv-slider
       v-model="useCameraSettingsStore().currentPipelineSettings.cornerDetectionAccuracyPercentage"
       class="pt-2"
       :slider-cols="interactiveCols"
diff --git a/photon-client/src/components/dashboard/tabs/TargetsTab.vue b/photon-client/src/components/dashboard/tabs/TargetsTab.vue
index 4c5fb23972..d0e31d5f13 100644
--- a/photon-client/src/components/dashboard/tabs/TargetsTab.vue
+++ b/photon-client/src/components/dashboard/tabs/TargetsTab.vue
@@ -8,11 +8,10 @@ import { useStateStore } from "@/stores/StateStore";
   <div>
     <v-row align="start" class="pb-4" style="height: 300px">
       <!-- Simple table height must be set here and in the CSS for the fixed-header to work -->
-      <v-simple-table fixed-header height="100%" dense dark>
+      <v-simple-table fixed-header dense dark>
         <template #default>
           <thead style="font-size: 1.25rem">
             <tr>
-              <th class="text-center">Target Count</th>
               <th
                 v-if="
                   useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag ||
@@ -45,8 +44,7 @@ import { useStateStore } from "@/stores/StateStore";
             </tr>
           </thead>
           <tbody>
-            <tr v-for="(target, index) in useStateStore().pipelineResults?.targets" :key="index">
-              <td>{{ index }}</td>
+            <tr v-for="(target, index) in useStateStore().currentPipelineResults?.targets" :key="index">
               <td
                 v-if="
                   useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag ||
@@ -64,7 +62,7 @@ import { useStateStore } from "@/stores/StateStore";
               <template v-else-if="useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled">
                 <td>{{ target.pose?.x.toFixed(2) }}&nbsp;m</td>
                 <td>{{ target.pose?.y.toFixed(2) }}&nbsp;m</td>
-                <td>{{ ((target.pose?.angle_z * 180.0) / Math.PI).toFixed(2) }}&deg;</td>
+                <td>{{ (((target.pose?.angle_z || 0) * 180.0) / Math.PI).toFixed(2) }}&deg;</td>
               </template>
               <template
                 v-if="
@@ -73,13 +71,37 @@ import { useStateStore } from "@/stores/StateStore";
                   useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled
                 "
               >
-                <td>{{ target.ambiguity?.toFixed(2) }}%</td>
+                <td>{{ target.ambiguity >= 0 ? target.ambiguity?.toFixed(2) + "%" : "(In Multi-Target)" }}</td>
               </template>
             </tr>
           </tbody>
         </template>
       </v-simple-table>
     </v-row>
+    <v-row
+      v-if="
+        useCameraSettingsStore().currentPipelineSettings.pipelineType === PipelineType.AprilTag &&
+        useCameraSettingsStore().currentPipelineSettings.doMultiTarget
+      "
+      align="start"
+      class="pb-4 white--text"
+    >
+      <v-card-subtitle>Multi-tag pose, field-to-camera</v-card-subtitle>
+      <v-simple-table fixed-header height="100%" dense dark>
+        <thead style="font-size: 1.25rem">
+          <th class="text-center">X meters</th>
+          <th class="text-center">Y meters</th>
+          <th class="text-center">Z Angle &theta;&deg;</th>
+          <th class="text-center">Tags</th>
+        </thead>
+        <tbody>
+          <td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }}</td>
+          <td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }}</td>
+          <td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z.toFixed(2) }}</td>
+          <td>{{ useStateStore().currentPipelineResults?.multitagResult?.fiducialIDsUsed }}</td>
+        </tbody>
+      </v-simple-table>
+    </v-row>
   </div>
 </template>
 
diff --git a/photon-client/src/components/dashboard/tabs/ThresholdTab.vue b/photon-client/src/components/dashboard/tabs/ThresholdTab.vue
index 5ac8deab95..bfcab45d66 100644
--- a/photon-client/src/components/dashboard/tabs/ThresholdTab.vue
+++ b/photon-client/src/components/dashboard/tabs/ThresholdTab.vue
@@ -1,8 +1,8 @@
 <script setup lang="ts">
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { computed, getCurrentInstance, onBeforeUnmount, onMounted } from "vue";
-import CvRangeSlider from "@/components/common/cv-range-slider.vue";
-import CvSwitch from "@/components/common/cv-switch.vue";
+import PvRangeSlider from "@/components/common/pv-range-slider.vue";
+import PvSwitch from "@/components/common/pv-switch.vue";
 import { useStateStore } from "@/stores/StateStore";
 import { ColorPicker, type HSV } from "@/lib/ColorPicker";
 
@@ -16,7 +16,7 @@ const averageHue = computed<number>(() => {
   return val;
 });
 
-// TODO fix cv-range-slider so that store access doesn't need to be deferred
+// TODO fix pv-range-slider so that store access doesn't need to be deferred
 const hsvHue = computed<[number, number]>({
   get: () => Object.values(useCameraSettingsStore().currentPipelineSettings.hsvHue) as [number, number],
   set: (v) => (useCameraSettingsStore().currentPipelineSettings.hsvHue = v)
@@ -135,7 +135,7 @@ const interactiveCols = computed(
 
 <template>
   <div class="threshold-modifiers" :style="{ '--averageHue': averageHue }">
-    <cv-range-slider
+    <pv-range-slider
       id="hue-slider"
       v-model="hsvHue"
       :class="useCameraSettingsStore().currentPipelineSettings.hueInverted ? 'inverted-slider' : 'normal-slider'"
@@ -147,7 +147,7 @@ const interactiveCols = computed(
       :inverted="useCameraSettingsStore().currentPipelineSettings.hueInverted"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ hsvHue: value }, false)"
     />
-    <cv-range-slider
+    <pv-range-slider
       id="sat-slider"
       v-model="hsvSaturation"
       class="normal-slider"
@@ -158,7 +158,7 @@ const interactiveCols = computed(
       :slider-cols="interactiveCols"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ hsvSaturation: value }, false)"
     />
-    <cv-range-slider
+    <pv-range-slider
       id="value-slider"
       v-model="hsvValue"
       class="normal-slider"
@@ -169,7 +169,7 @@ const interactiveCols = computed(
       :slider-cols="interactiveCols"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ hsvValue: value }, false)"
     />
-    <cv-switch
+    <pv-switch
       v-model="useCameraSettingsStore().currentPipelineSettings.hueInverted"
       label="Invert Hue"
       :switch-cols="interactiveCols"
diff --git a/photon-client/src/components/settings/ApriltagControlCard.vue b/photon-client/src/components/settings/ApriltagControlCard.vue
new file mode 100644
index 0000000000..033dbd5474
--- /dev/null
+++ b/photon-client/src/components/settings/ApriltagControlCard.vue
@@ -0,0 +1,93 @@
+<script setup lang="ts">
+import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
+import { Euler, Quaternion as ThreeQuat } from "three";
+import type { Quaternion } from "@/types/PhotonTrackingTypes";
+
+const quatToEuler = (quat: Quaternion): Euler => {
+  const three_quat = new ThreeQuat(quat.X, quat.Y, quat.Z, quat.W);
+  return new Euler().setFromQuaternion(three_quat, "ZYX");
+};
+
+// Convert from radians to degrees.
+const degrees = (radians: number): number => (radians * 180) / Math.PI;
+</script>
+
+<template>
+  <v-card dark class="mb-3 pr-6 pb-3" style="background-color: #006492">
+    <v-card-title>Apriltag Layout</v-card-title>
+    <div class="ml-5">
+      <p>Field width: {{ useSettingsStore().currentFieldLayout.field.width.toFixed(2) }} meters</p>
+      <p>Field length: {{ useSettingsStore().currentFieldLayout.field.length.toFixed(2) }} meters</p>
+
+      <!-- Simple table height must be set here and in the CSS for the fixed-header to work -->
+      <v-simple-table fixed-header height="100%" dense dark>
+        <template #default>
+          <thead style="font-size: 1.25rem">
+            <tr>
+              <th class="text-center">ID</th>
+              <th class="text-center">X, meters</th>
+              <th class="text-center">Y, meters</th>
+              <th class="text-center">Z, meters</th>
+              <th class="text-center">θ<sub>x</sub> angle, &deg;</th>
+              <th class="text-center">θ<sub>y</sub> angle, &deg;</th>
+              <th class="text-center">θ<sub>z</sub> angle, &deg;</th>
+            </tr>
+          </thead>
+          <tbody>
+            <tr v-for="(tag, index) in useSettingsStore().currentFieldLayout.tags" :key="index">
+              <td>{{ tag.ID }}</td>
+              <td v-for="(val, idx) in Object.values(tag.pose.translation).slice(0, 3).map(degrees)" :key="idx">
+                {{ val.toFixed(2) }}
+              </td>
+              <td
+                v-for="(val, idx) in Object.values(quatToEuler(tag.pose.rotation.quaternion)).slice(0, 3).map(degrees)"
+                :key="idx"
+              >
+                {{ val.toFixed(2) }}
+              </td>
+            </tr>
+          </tbody>
+        </template>
+      </v-simple-table>
+    </div>
+  </v-card>
+</template>
+
+<style scoped lang="scss">
+.v-data-table {
+  width: 100%;
+  height: 100%;
+  text-align: center;
+  background-color: #006492 !important;
+
+  th,
+  td {
+    background-color: #006492 !important;
+    font-size: 1rem !important;
+  }
+
+  td {
+    font-family: monospace !important;
+  }
+
+  tbody :hover td {
+    background-color: #005281 !important;
+  }
+
+  ::-webkit-scrollbar {
+    width: 0;
+    height: 0.55em;
+    border-radius: 5px;
+  }
+
+  ::-webkit-scrollbar-track {
+    -webkit-box-shadow: inset 0 0 6px rgba(0, 0, 0, 0.3);
+    border-radius: 10px;
+  }
+
+  ::-webkit-scrollbar-thumb {
+    background-color: #ffd843;
+    border-radius: 10px;
+  }
+}
+</style>
diff --git a/photon-client/src/components/settings/DeviceControlCard.vue b/photon-client/src/components/settings/DeviceControlCard.vue
index 29ae2e92d0..157bb6b7b0 100644
--- a/photon-client/src/components/settings/DeviceControlCard.vue
+++ b/photon-client/src/components/settings/DeviceControlCard.vue
@@ -1,7 +1,7 @@
 <script setup lang="ts">
 import { inject, ref } from "vue";
 import { useStateStore } from "@/stores/StateStore";
-import CvSelect from "@/components/common/cv-select.vue";
+import PvSelect from "@/components/common/pv-select.vue";
 import axios from "axios";
 
 const restartProgram = () => {
@@ -63,19 +63,18 @@ const offlineUpdate = ref();
 const openOfflineUpdatePrompt = () => {
   offlineUpdate.value.click();
 };
-const handleOfflineUpdate = (payload: Event) => {
+const handleOfflineUpdate = (payload: Event & { target: (EventTarget & HTMLInputElement) | null }) => {
+  if (payload.target === null || !payload.target.files) return;
+
+  const formData = new FormData();
+  formData.append("jarData", payload.target.files[0]);
+
   useStateStore().showSnackbarMessage({
     message: "New Software Upload in Progress...",
     color: "secondary",
     timeout: -1
   });
 
-  const formData = new FormData();
-
-  if (payload.target == null || !payload.target?.files) return;
-  const files: FileList = payload.target.files as FileList;
-  formData.append("jarData", files[0]);
-
   axios
     .post("/utils/offlineUpdate", formData, {
       headers: { "Content-Type": "multipart/form-data" },
@@ -136,23 +135,20 @@ enum ImportType {
   AllSettings,
   HardwareConfig,
   HardwareSettings,
-  NetworkConfig
+  NetworkConfig,
+  ApriltagFieldLayout
 }
-
 const showImportDialog = ref(false);
 const importType = ref<ImportType | number>(-1);
-const importFile = ref(null);
+const importFile = ref<File | null>(null);
 const handleSettingsImport = () => {
   if (importType.value === -1 || importFile.value === null) return;
 
   const formData = new FormData();
   formData.append("data", importFile.value);
 
-  let settingsEndpoint;
+  let settingsEndpoint: string;
   switch (importType.value) {
-    case ImportType.AllSettings:
-      settingsEndpoint = "";
-      break;
     case ImportType.HardwareConfig:
       settingsEndpoint = "/hardwareConfig";
       break;
@@ -162,6 +158,13 @@ const handleSettingsImport = () => {
     case ImportType.NetworkConfig:
       settingsEndpoint = "/networkConfig";
       break;
+    case ImportType.ApriltagFieldLayout:
+      settingsEndpoint = "/aprilTagFieldLayout";
+      break;
+    default:
+    case ImportType.AllSettings:
+      settingsEndpoint = "";
+      break;
   }
 
   axios
@@ -246,20 +249,27 @@ const handleSettingsImport = () => {
               <v-card-text>
                 Upload and apply previously saved or exported PhotonVision settings to this device
                 <v-row class="mt-6 ml-4">
-                  <cv-select
+                  <pv-select
                     v-model="importType"
                     label="Type"
                     tooltip="Select the type of settings file you are trying to upload"
-                    :items="['All Settings', 'Hardware Config', 'Hardware Settings', 'Network Config']"
+                    :items="[
+                      'All Settings',
+                      'Hardware Config',
+                      'Hardware Settings',
+                      'Network Config',
+                      'Apriltag Layout'
+                    ]"
                     :select-cols="10"
+                    style="width: 100%"
                   />
                 </v-row>
                 <v-row class="mt-6 ml-4 mr-8">
                   <v-file-input
+                    v-model="importFile"
                     :disabled="importType === -1"
                     :error-messages="importType === -1 ? 'Settings type not selected' : ''"
                     :accept="importType === ImportType.AllSettings ? '.zip' : '.json'"
-                    @change="(file) => (importFile = file)"
                   />
                 </v-row>
                 <v-row
diff --git a/photon-client/src/components/settings/LEDControlCard.vue b/photon-client/src/components/settings/LEDControlCard.vue
index 2a8a86c497..646c809403 100644
--- a/photon-client/src/components/settings/LEDControlCard.vue
+++ b/photon-client/src/components/settings/LEDControlCard.vue
@@ -1,5 +1,5 @@
 <script setup lang="ts">
-import CvSlider from "@/components/common/cv-slider.vue";
+import PvSlider from "@/components/common/pv-slider.vue";
 import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
 </script>
 
@@ -7,7 +7,7 @@ import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
   <v-card dark class="mb-3 pr-6 pb-3" style="background-color: #006492">
     <v-card-title>LED Control</v-card-title>
     <div class="ml-5">
-      <cv-slider
+      <pv-slider
         v-model="useSettingsStore().lighting.brightness"
         label="Brightness"
         class="pt-2"
diff --git a/photon-client/src/components/settings/MetricsCard.vue b/photon-client/src/components/settings/MetricsCard.vue
index bc86036409..e79b3f031d 100644
--- a/photon-client/src/components/settings/MetricsCard.vue
+++ b/photon-client/src/components/settings/MetricsCard.vue
@@ -2,7 +2,7 @@
 import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
 import { computed, onBeforeMount, ref } from "vue";
 import { useStateStore } from "@/stores/StateStore";
-import CvIcon from "@/components/common/cv-icon.vue";
+import PvIcon from "@/components/common/pv-icon.vue";
 
 interface MetricItem {
   header: string;
@@ -72,12 +72,12 @@ const fetchMetrics = () => {
       if (error.request) {
         useStateStore().showSnackbarMessage({
           color: "error",
-          message: "Unable to fetch Metrics! The backend didn't respond."
+          message: "Unable to fetch metrics! The backend didn't respond."
         });
       } else {
         useStateStore().showSnackbarMessage({
           color: "error",
-          message: "An error occurred while trying to fetch Metrics."
+          message: "An error occurred while trying to fetch metrics."
         });
       }
     })
@@ -100,7 +100,7 @@ onBeforeMount(() => {
   <v-card dark class="mb-3 pr-6 pb-3" style="background-color: #006492">
     <v-card-title style="display: flex; justify-content: space-between">
       <span>Stats</span>
-      <cv-icon icon-name="mdi-reload" color="white" tooltip="Reload Metrics" hover @click="fetchMetrics" />
+      <pv-icon icon-name="mdi-reload" color="white" tooltip="Reload Metrics" hover @click="fetchMetrics" />
     </v-card-title>
     <v-row class="pt-2 pa-4 ma-0 ml-5 pb-1">
       <v-card-subtitle class="ma-0 pa-0 pb-2" style="font-size: 16px"> General Metrics </v-card-subtitle>
diff --git a/photon-client/src/components/settings/NetworkingCard.vue b/photon-client/src/components/settings/NetworkingCard.vue
index 93bb330165..0a81f6b39b 100644
--- a/photon-client/src/components/settings/NetworkingCard.vue
+++ b/photon-client/src/components/settings/NetworkingCard.vue
@@ -1,10 +1,10 @@
 <script setup lang="ts">
 import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
 import { computed, ref } from "vue";
-import CvInput from "@/components/common/cv-input.vue";
-import CvRadio from "@/components/common/cv-radio.vue";
-import CvSwitch from "@/components/common/cv-switch.vue";
-import CvSelect from "@/components/common/cv-select.vue";
+import PvInput from "@/components/common/pv-input.vue";
+import PvRadio from "@/components/common/pv-radio.vue";
+import PvSwitch from "@/components/common/pv-switch.vue";
+import PvSelect from "@/components/common/pv-select.vue";
 import { NetworkConnectionType } from "@/types/SettingTypes";
 import { useStateStore } from "@/stores/StateStore";
 
@@ -89,7 +89,7 @@ const currentNetworkInterfaceIndex = computed<number>({
     <v-card-title>Networking</v-card-title>
     <div class="ml-5">
       <v-form ref="form" v-model="settingsValid">
-        <cv-input
+        <pv-input
           v-model="useSettingsStore().network.ntServerAddress"
           label="Team Number/NetworkTables Server Address"
           tooltip="Enter the Team Number or the IP address of the NetworkTables Server"
@@ -114,7 +114,7 @@ const currentNetworkInterfaceIndex = computed<number>({
         >
           The NetworkTables Server Address is not set or is invalid. NetworkTables is unable to connect.
         </v-banner>
-        <cv-radio
+        <pv-radio
           v-model="useSettingsStore().network.connectionType"
           label="IP Assignment Mode"
           tooltip="DHCP will make the radio (router) automatically assign an IP address; this may result in an IP address that changes across reboots. Static IP assignment means that you pick the IP address and it won't change."
@@ -122,7 +122,7 @@ const currentNetworkInterfaceIndex = computed<number>({
           :list="['DHCP', 'Static']"
           :disabled="!(useSettingsStore().network.shouldManage && useSettingsStore().network.canManage)"
         />
-        <cv-input
+        <pv-input
           v-if="useSettingsStore().network.connectionType === NetworkConnectionType.Static"
           v-model="useSettingsStore().network.staticIp"
           :input-cols="12 - 4"
@@ -130,7 +130,7 @@ const currentNetworkInterfaceIndex = computed<number>({
           :rules="[(v) => isValidIPv4(v) || 'Invalid IPv4 address']"
           :disabled="!(useSettingsStore().network.shouldManage && useSettingsStore().network.canManage)"
         />
-        <cv-input
+        <pv-input
           v-model="useSettingsStore().network.hostname"
           label="Hostname"
           :input-cols="12 - 4"
@@ -139,7 +139,7 @@ const currentNetworkInterfaceIndex = computed<number>({
         />
         <v-divider class="pb-3" />
         <span style="font-weight: 700">Advanced Networking</span>
-        <cv-switch
+        <pv-switch
           v-model="useSettingsStore().network.shouldManage"
           :disabled="!useSettingsStore().network.canManage"
           label="Manage Device Networking"
@@ -147,7 +147,7 @@ const currentNetworkInterfaceIndex = computed<number>({
           :label-cols="4"
           class="pt-2"
         />
-        <cv-select
+        <pv-select
           v-model="currentNetworkInterfaceIndex"
           label="NetworkManager interface"
           :disabled="!(useSettingsStore().network.shouldManage && useSettingsStore().network.canManage)"
@@ -168,7 +168,7 @@ const currentNetworkInterfaceIndex = computed<number>({
         >
           Photon cannot detect any wired connections! Please send program logs to the developers for help.
         </v-banner>
-        <cv-switch
+        <pv-switch
           v-model="useSettingsStore().network.runNTServer"
           label="Run NetworkTables Server (Debugging Only)"
           tooltip="If enabled, this device will create a NT server. This is useful for home debugging, but should be disabled on-robot."
diff --git a/photon-client/src/stores/StateStore.ts b/photon-client/src/stores/StateStore.ts
index 6c2a9957e8..3f51b56fb2 100644
--- a/photon-client/src/stores/StateStore.ts
+++ b/photon-client/src/stores/StateStore.ts
@@ -24,7 +24,7 @@ interface StateStore {
   logMessages: LogMessage[];
   currentCameraIndex: number;
 
-  pipelineResults?: PipelineResult;
+  backendResults: Record<string, PipelineResult>;
 
   colorPickingMode: boolean;
 
@@ -58,7 +58,7 @@ export const useStateStore = defineStore("state", {
       logMessages: [],
       currentCameraIndex: 0,
 
-      pipelineResults: undefined,
+      backendResults: {},
 
       colorPickingMode: false,
 
@@ -77,6 +77,11 @@ export const useStateStore = defineStore("state", {
       }
     };
   },
+  getters: {
+    currentPipelineResults(): PipelineResult | undefined {
+      return this.backendResults[this.currentCameraIndex.toString()];
+    }
+  },
   actions: {
     setSidebarFolded(value: boolean) {
       this.sidebarFolded = value;
@@ -95,12 +100,11 @@ export const useStateStore = defineStore("state", {
         clients: data.clients
       };
     },
-    updatePipelineResultsFromWebsocket(data: WebsocketPipelineResultUpdate) {
-      for (const cameraIndex in data) {
-        if (parseInt(cameraIndex) === this.currentCameraIndex) {
-          this.pipelineResults = data[cameraIndex];
-        }
-      }
+    updateBackendResultsFromWebsocket(data: WebsocketPipelineResultUpdate) {
+      this.backendResults = {
+        ...this.backendResults,
+        ...data
+      };
     },
     updateCalibrationStateValuesFromWebsocket(data: WebsocketCalibrationData) {
       this.calibrationData = {
diff --git a/photon-client/src/stores/settings/CameraSettingsStore.ts b/photon-client/src/stores/settings/CameraSettingsStore.ts
index 673862771e..861b192ec3 100644
--- a/photon-client/src/stores/settings/CameraSettingsStore.ts
+++ b/photon-client/src/stores/settings/CameraSettingsStore.ts
@@ -173,7 +173,9 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
       settings: Partial<ActivePipelineSettings>,
       cameraIndex: number = useStateStore().currentCameraIndex
     ) {
-      Object.assign(this.cameras[cameraIndex].pipelineSettings, settings);
+      Object.entries(settings).forEach(([k, v]) => {
+        this.cameras[cameraIndex].pipelineSettings[k] = v;
+      });
     },
     /**
      * Change the nickname of the currently selected pipeline of the provided camera.
@@ -357,12 +359,35 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
     /**
      * Take a snapshot for the calibration processes
      *
-     * @param takeSnapshot whether or not to take a snapshot. Defaults to true
      * @param cameraIndex the index of the camera that is currently in the calibration process
      */
-    takeCalibrationSnapshot(takeSnapshot = true, cameraIndex: number = useStateStore().currentCameraIndex) {
+    takeCalibrationSnapshot(cameraIndex: number = useStateStore().currentCameraIndex) {
       const payload = {
-        takeCalibrationSnapshot: takeSnapshot,
+        takeCalibrationSnapshot: true,
+        cameraIndex: cameraIndex
+      };
+      useStateStore().websocket?.send(payload, true);
+    },
+    /**
+     * Save a snapshot of the input frame of the camera.
+     *
+     * @param cameraIndex the index of the camera
+     */
+    saveInputSnapshot(cameraIndex: number = useStateStore().currentCameraIndex) {
+      const payload = {
+        saveInputSnapshot: true,
+        cameraIndex: cameraIndex
+      };
+      useStateStore().websocket?.send(payload, true);
+    },
+    /**
+     * Save a snapshot of the output frame of the camera.
+     *
+     * @param cameraIndex the index of the camera
+     */
+    saveOutputSnapshot(cameraIndex: number = useStateStore().currentCameraIndex) {
+      const payload = {
+        saveOutputSnapshot: true,
         cameraIndex: cameraIndex
       };
       useStateStore().websocket?.send(payload, true);
diff --git a/photon-client/src/stores/settings/GeneralSettingsStore.ts b/photon-client/src/stores/settings/GeneralSettingsStore.ts
index 2e5cd3b89b..5202d8626e 100644
--- a/photon-client/src/stores/settings/GeneralSettingsStore.ts
+++ b/photon-client/src/stores/settings/GeneralSettingsStore.ts
@@ -10,12 +10,14 @@ import { NetworkConnectionType } from "@/types/SettingTypes";
 import { useStateStore } from "@/stores/StateStore";
 import axios from "axios";
 import type { WebsocketSettingsUpdate } from "@/types/WebsocketDataTypes";
+import type { AprilTagFieldLayout } from "@/types/PhotonTrackingTypes";
 
 interface GeneralSettingsStore {
   general: GeneralSettings;
   network: NetworkSettings;
   lighting: LightingSettings;
   metrics: MetricData;
+  currentFieldLayout: AprilTagFieldLayout;
 }
 
 export const useSettingsStore = defineStore("settings", {
@@ -55,6 +57,13 @@ export const useSettingsStore = defineStore("settings", {
       cpuThr: undefined,
       cpuUptime: undefined,
       diskUtilPct: undefined
+    },
+    currentFieldLayout: {
+      field: {
+        length: 16.4592,
+        width: 8.2296
+      },
+      tags: []
     }
   }),
   getters: {
@@ -91,6 +100,7 @@ export const useSettingsStore = defineStore("settings", {
       };
       this.lighting = data.lighting;
       this.network = data.networkSettings;
+      this.currentFieldLayout = data.atfl;
     },
     saveGeneralSettings() {
       const payload: Required<ConfigurableNetworkSettings> = {
diff --git a/photon-client/src/types/PhotonTrackingTypes.ts b/photon-client/src/types/PhotonTrackingTypes.ts
index 0bd3028c4d..ee19240ffa 100644
--- a/photon-client/src/types/PhotonTrackingTypes.ts
+++ b/photon-client/src/types/PhotonTrackingTypes.ts
@@ -1,12 +1,39 @@
-export interface Pose {
+export interface Transform3d {
   x: number;
   y: number;
   z: number;
-  angle_z: number;
   qw: number;
   qx: number;
   qy: number;
   qz: number;
+  angle_z: number;
+}
+
+export interface Quaternion {
+  X: number;
+  Y: number;
+  Z: number;
+  W: number;
+}
+
+export interface AprilTagFieldLayout {
+  field: {
+    length: number;
+    width: number;
+  };
+  tags: {
+    ID: number;
+    pose: {
+      translation: {
+        x: number;
+        y: number;
+        z: number;
+      };
+      rotation: {
+        quaternion: Quaternion;
+      };
+    };
+  }[];
 }
 
 export interface PhotonTarget {
@@ -19,11 +46,19 @@ export interface PhotonTarget {
   // -1 if not set
   fiducialId: number;
   // undefined if 3d isn't enabled
-  pose?: Pose;
+  pose?: Transform3d;
+}
+
+export interface MultitagResult {
+  bestTransform: Transform3d;
+  bestReprojectionError: number;
+  fiducialIDsUsed: number[];
 }
 
 export interface PipelineResult {
   fps: number;
   latency: number;
   targets: PhotonTarget[];
+  // undefined if multitag failed or non-tag pipeline
+  multitagResult?: MultitagResult;
 }
diff --git a/photon-client/src/types/PipelineTypes.ts b/photon-client/src/types/PipelineTypes.ts
index 7b2a7d73ad..19dcd8dad1 100644
--- a/photon-client/src/types/PipelineTypes.ts
+++ b/photon-client/src/types/PipelineTypes.ts
@@ -214,6 +214,8 @@ export interface AprilTagPipelineSettings extends PipelineSettings {
   debug: boolean;
   threads: number;
   tagFamily: AprilTagFamily;
+  doMultiTarget: boolean;
+  doSingleTargetAlways: boolean;
 }
 export type ConfigurableAprilTagPipelineSettings = Partial<
   Omit<AprilTagPipelineSettings, "pipelineType" | "hammingDist" | "debug">
@@ -236,7 +238,9 @@ export const DefaultAprilTagPipelineSettings: AprilTagPipelineSettings = {
   refineEdges: true,
   debug: false,
   threads: 4,
-  tagFamily: AprilTagFamily.Family16h5
+  tagFamily: AprilTagFamily.Family16h5,
+  doMultiTarget: false,
+  doSingleTargetAlways: false
 };
 
 export interface ArucoPipelineSettings extends PipelineSettings {
@@ -279,6 +283,7 @@ export type ActivePipelineSettings =
   | ColoredShapePipelineSettings
   | AprilTagPipelineSettings
   | ArucoPipelineSettings;
+
 export type ActiveConfigurablePipelineSettings =
   | ConfigurableReflectivePipelineSettings
   | ConfigurableColoredShapePipelineSettings
diff --git a/photon-client/src/types/WebsocketDataTypes.ts b/photon-client/src/types/WebsocketDataTypes.ts
index 0a31efb769..0114e19e79 100644
--- a/photon-client/src/types/WebsocketDataTypes.ts
+++ b/photon-client/src/types/WebsocketDataTypes.ts
@@ -1,5 +1,6 @@
 import type { GeneralSettings, LightingSettings, LogLevel, MetricData, NetworkSettings } from "@/types/SettingTypes";
 import type { ActivePipelineSettings } from "@/types/PipelineTypes";
+import type { AprilTagFieldLayout, PipelineResult } from "@/types/PhotonTrackingTypes";
 
 export interface WebsocketLogMessage {
   logMessage: {
@@ -11,6 +12,7 @@ export interface WebsocketSettingsUpdate {
   general: Required<GeneralSettings>;
   lighting: Required<LightingSettings>;
   networkSettings: NetworkSettings;
+  atfl: AprilTagFieldLayout;
 }
 
 export interface WebsocketNumberPair {
@@ -60,31 +62,9 @@ export interface WebsocketNTUpdate {
   address?: string;
   clients?: number;
 }
-export type WebsocketPipelineResultUpdate = Record<
-  number,
-  {
-    fps: number;
-    latency: number;
-    targets: {
-      yaw: number;
-      pitch: number;
-      skew: number;
-      area: number;
-      ambiguity: number;
-      fiducialId: number;
-      pose: {
-        angle_z: number;
-        qw: number;
-        qx: number;
-        x: number;
-        qy: number;
-        y: number;
-        qz: number;
-        z: number;
-      };
-    }[];
-  }
->;
+
+// key is the index of the camera, value is that camera's result
+export type WebsocketPipelineResultUpdate = Record<string, PipelineResult>;
 
 export interface WebsocketCalibrationData {
   patternWidth: number;
@@ -106,7 +86,7 @@ export interface IncomingWebsocketData {
   updatePipelineResult?: WebsocketPipelineResultUpdate;
   networkInfo?: {
     possibleRios: string[];
-    deviceips: string[];
+    deviceIps: string[];
   };
   mutatePipelineSettings?: Partial<ActivePipelineSettings>;
   cameraIndex?: number; // Sent when mutating pipeline settings to check against currently active
diff --git a/photon-client/src/views/GeneralSettingsView.vue b/photon-client/src/views/GeneralSettingsView.vue
index 4cc5d0206d..edd3e67edd 100644
--- a/photon-client/src/views/GeneralSettingsView.vue
+++ b/photon-client/src/views/GeneralSettingsView.vue
@@ -4,6 +4,7 @@ import DeviceControlCard from "@/components/settings/DeviceControlCard.vue";
 import NetworkingCard from "@/components/settings/NetworkingCard.vue";
 import LightingControlCard from "@/components/settings/LEDControlCard.vue";
 import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
+import ApriltagControlCard from "@/components/settings/ApriltagControlCard.vue";
 </script>
 
 <template>
@@ -12,5 +13,6 @@ import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
     <DeviceControlCard />
     <NetworkingCard />
     <LightingControlCard v-if="useSettingsStore().lighting.supported" />
+    <ApriltagControlCard />
   </div>
 </template>
diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java
index 48a6e011bf..1d87dd17ab 100644
--- a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java
+++ b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java
@@ -255,6 +255,10 @@ public boolean saveUploadedNetworkConfig(Path uploadPath) {
         return m_provider.saveUploadedNetworkConfig(uploadPath);
     }
 
+    public boolean saveUploadedAprilTagFieldLayout(Path uploadPath) {
+        return m_provider.saveUploadedAprilTagFieldLayout(uploadPath);
+    }
+
     public void requestSave() {
         logger.trace("Requesting save...");
         saveRequestTimestamp = System.currentTimeMillis();
diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigProvider.java
index a434df0e2b..9964d0ca93 100644
--- a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigProvider.java
+++ b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigProvider.java
@@ -39,4 +39,6 @@ public void clearConfig() {
     public abstract boolean saveUploadedHardwareSettings(Path uploadPath);
 
     public abstract boolean saveUploadedNetworkConfig(Path uploadPath);
+
+    public abstract boolean saveUploadedAprilTagFieldLayout(Path uploadPath);
 }
diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java
index 6dca0ddcb2..74d5d472fe 100644
--- a/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java
+++ b/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java
@@ -18,8 +18,11 @@
 package org.photonvision.common.configuration;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import java.io.File;
 import java.io.IOException;
+import java.io.UncheckedIOException;
 import java.nio.file.Files;
 import java.nio.file.Path;
 import java.text.DateFormat;
@@ -45,12 +48,14 @@ class LegacyConfigProvider extends ConfigProvider {
     public static final String HW_CFG_FNAME = "hardwareConfig.json";
     public static final String HW_SET_FNAME = "hardwareSettings.json";
     public static final String NET_SET_FNAME = "networkSettings.json";
+    public static final String ATFL_SET_FNAME = "apriltagFieldLayout.json";
 
     private PhotonConfiguration config;
     private final File hardwareConfigFile;
     private final File hardwareSettingsFile;
     private final File networkConfigFile;
     private final File camerasFolder;
+    private final File apriltagFieldLayoutFile;
 
     final File configDirectoryFile;
 
@@ -86,6 +91,8 @@ protected LegacyConfigProvider(Path configDirectoryFile) {
                 new File(Path.of(configDirectoryFile.toString(), HW_SET_FNAME).toUri());
         this.networkConfigFile =
                 new File(Path.of(configDirectoryFile.toString(), NET_SET_FNAME).toUri());
+        this.apriltagFieldLayoutFile =
+                new File(Path.of(configDirectoryFile.toString(), ATFL_SET_FNAME).toUri());
         this.camerasFolder = new File(Path.of(configDirectoryFile.toString(), "cameras").toUri());
 
         settingsSaveThread = new Thread(this::saveAndWriteTask);
@@ -116,6 +123,7 @@ public void load() {
         HardwareConfig hardwareConfig;
         HardwareSettings hardwareSettings;
         NetworkConfig networkConfig;
+        AprilTagFieldLayout atfl = null;
 
         if (hardwareConfigFile.exists()) {
             try {
@@ -175,11 +183,38 @@ public void load() {
             }
         }
 
+        if (apriltagFieldLayoutFile.exists()) {
+            try {
+                atfl =
+                        JacksonUtils.deserialize(apriltagFieldLayoutFile.toPath(), AprilTagFieldLayout.class);
+                if (atfl == null) {
+                    logger.error("Could not deserialize apriltag field layout! (still null)");
+                }
+            } catch (IOException e) {
+                logger.error("Could not deserialize apriltag field layout!", e);
+                atfl = null; // not required, nice to be explicit
+            }
+        }
+        if (atfl == null) {
+            logger.info("Loading default apriltags for 2023 field...");
+            try {
+                atfl = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+            } catch (UncheckedIOException e) {
+                logger.error("Error loading WPILib field", e);
+                atfl = null;
+            }
+            if (atfl == null) {
+                // what do we even do here lmao -- wpilib built-in should always work
+                logger.error("Field layout is *still* null??????");
+                atfl = new AprilTagFieldLayout(List.of(), 1, 1);
+            }
+        }
+
         HashMap<String, CameraConfiguration> cameraConfigurations = loadCameraConfigs();
 
         this.config =
                 new PhotonConfiguration(
-                        hardwareConfig, hardwareSettings, networkConfig, cameraConfigurations);
+                        hardwareConfig, hardwareSettings, networkConfig, atfl, cameraConfigurations);
     }
 
     @Override
@@ -400,6 +435,10 @@ public Path getNetworkConfigFile() {
         return this.networkConfigFile.toPath();
     }
 
+    public Path getAprilTagFieldLayoutFile() {
+        return this.apriltagFieldLayoutFile.toPath();
+    }
+
     @Override
     public boolean saveUploadedHardwareConfig(Path uploadPath) {
         return FileUtils.replaceFile(uploadPath, this.getHardwareConfigFile());
@@ -415,6 +454,11 @@ public boolean saveUploadedNetworkConfig(Path uploadPath) {
         return FileUtils.replaceFile(uploadPath, this.getNetworkConfigFile());
     }
 
+    @Override
+    public boolean saveUploadedAprilTagFieldLayout(Path uploadPath) {
+        return FileUtils.replaceFile(uploadPath, this.getAprilTagFieldLayoutFile());
+    }
+
     public void requestSave() {
         logger.trace("Requesting save...");
         saveRequestTimestamp = System.currentTimeMillis();
diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
index 262cdd7111..fac672636c 100644
--- a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
+++ b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
@@ -17,6 +17,7 @@
 
 package org.photonvision.common.configuration;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import java.util.Collection;
 import java.util.HashMap;
 import java.util.List;
@@ -31,33 +32,40 @@
 import org.photonvision.vision.processes.VisionModuleManager;
 import org.photonvision.vision.processes.VisionSource;
 
-// TODO rename this class
 public class PhotonConfiguration {
     private final HardwareConfig hardwareConfig;
     private final HardwareSettings hardwareSettings;
     private NetworkConfig networkConfig;
-    private final HashMap<String, CameraConfiguration> cameraConfigurations;
+    private AprilTagFieldLayout atfl;
+    private HashMap<String, CameraConfiguration> cameraConfigurations;
 
     public PhotonConfiguration(
             HardwareConfig hardwareConfig,
             HardwareSettings hardwareSettings,
-            NetworkConfig networkConfig) {
-        this(hardwareConfig, hardwareSettings, networkConfig, new HashMap<>());
+            NetworkConfig networkConfig,
+            AprilTagFieldLayout atfl) {
+        this(hardwareConfig, hardwareSettings, networkConfig, atfl, new HashMap<>());
     }
 
     public PhotonConfiguration(
             HardwareConfig hardwareConfig,
             HardwareSettings hardwareSettings,
             NetworkConfig networkConfig,
+            AprilTagFieldLayout atfl,
             HashMap<String, CameraConfiguration> cameraConfigurations) {
         this.hardwareConfig = hardwareConfig;
         this.hardwareSettings = hardwareSettings;
         this.networkConfig = networkConfig;
         this.cameraConfigurations = cameraConfigurations;
+        this.atfl = atfl;
     }
 
     public PhotonConfiguration() {
-        this(new HardwareConfig(), new HardwareSettings(), new NetworkConfig());
+        this(
+                new HardwareConfig(),
+                new HardwareSettings(),
+                new NetworkConfig(),
+                new AprilTagFieldLayout(List.of(), 0, 0));
     }
 
     public HardwareConfig getHardwareConfig() {
@@ -72,6 +80,14 @@ public HardwareSettings getHardwareSettings() {
         return hardwareSettings;
     }
 
+    public AprilTagFieldLayout getApriltagFieldLayout() {
+        return atfl;
+    }
+
+    public void setApriltagFieldLayout(AprilTagFieldLayout atfl) {
+        this.atfl = atfl;
+    }
+
     public void setNetworkConfig(NetworkConfig networkConfig) {
         this.networkConfig = networkConfig;
     }
@@ -115,7 +131,7 @@ public Map<String, Object> toHashMap() {
         lightingConfig.brightness = hardwareSettings.ledBrightnessPercentage;
         lightingConfig.supported = !hardwareConfig.ledPins.isEmpty();
         settingsSubmap.put("lighting", SerializationUtils.objectToHashMap(lightingConfig));
-
+        // General Settings
         var generalSubmap = new HashMap<String, Object>();
         generalSubmap.put("version", PhotonVersion.versionString);
         generalSubmap.put(
@@ -126,8 +142,17 @@ public Map<String, Object> toHashMap() {
         generalSubmap.put("hardwareModel", hardwareConfig.deviceName);
         generalSubmap.put("hardwarePlatform", Platform.getPlatformName());
         settingsSubmap.put("general", generalSubmap);
+        // AprilTagFieldLayout
+        settingsSubmap.put("atfl", this.atfl);
 
+        map.put(
+                "cameraSettings",
+                VisionModuleManager.getInstance().getModules().stream()
+                        .map(VisionModule::toUICameraConfig)
+                        .map(SerializationUtils::objectToHashMap)
+                        .collect(Collectors.toList()));
         map.put("settings", settingsSubmap);
+
         return map;
     }
 
diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java
index df63b27588..8a0c755ad8 100644
--- a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java
+++ b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java
@@ -17,8 +17,11 @@
 
 package org.photonvision.common.configuration;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import java.io.File;
 import java.io.IOException;
+import java.io.UncheckedIOException;
 import java.nio.file.Files;
 import java.nio.file.Path;
 import java.sql.*;
@@ -53,6 +56,7 @@ static class TableKeys {
         static final String NETWORK_CONFIG = "networkConfig";
         static final String HARDWARE_CONFIG = "hardwareConfig";
         static final String HARDWARE_SETTINGS = "hardwareSettings";
+        static final String ATFL_CONFIG_FILE = "apriltagFieldLayout";
     }
 
     private static final String dbName = "photon.sqlite";
@@ -201,6 +205,7 @@ public void load() {
             HardwareConfig hardwareConfig;
             HardwareSettings hardwareSettings;
             NetworkConfig networkConfig;
+            AprilTagFieldLayout atfl;
 
             try {
                 hardwareConfig =
@@ -229,6 +234,25 @@ public void load() {
                 networkConfig = new NetworkConfig();
             }
 
+            try {
+                atfl =
+                        JacksonUtils.deserialize(
+                                getOneConfigFile(conn, TableKeys.ATFL_CONFIG_FILE), AprilTagFieldLayout.class);
+            } catch (IOException e) {
+                logger.error("Could not deserialize apriltag layout! Loading defaults");
+                try {
+                    atfl = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+                } catch (UncheckedIOException e2) {
+                    logger.error("Error loading WPILib field", e);
+                    atfl = null;
+                }
+                if (atfl == null) {
+                    // what do we even do here lmao -- wpilib should always work
+                    logger.error("Field layout is *still* null??????");
+                    atfl = new AprilTagFieldLayout(List.of(), 1, 1);
+                }
+            }
+
             var cams = loadCameraConfigs(conn);
 
             try {
@@ -237,7 +261,8 @@ public void load() {
                 logger.error("SQL Err closing connection while loading: ", e);
             }
 
-            this.config = new PhotonConfiguration(hardwareConfig, hardwareSettings, networkConfig, cams);
+            this.config =
+                    new PhotonConfiguration(hardwareConfig, hardwareSettings, networkConfig, atfl, cams);
         }
     }
 
@@ -416,6 +441,11 @@ public boolean saveUploadedNetworkConfig(Path uploadPath) {
         return saveOneFile(TableKeys.NETWORK_CONFIG, uploadPath);
     }
 
+    @Override
+    public boolean saveUploadedAprilTagFieldLayout(Path uploadPath) {
+        return saveOneFile(TableKeys.ATFL_CONFIG_FILE, uploadPath);
+    }
+
     private HashMap<String, CameraConfiguration> loadCameraConfigs(Connection conn) {
         HashMap<String, CameraConfiguration> loadedConfigurations = new HashMap<>();
 
diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
index 56bae44bd7..d985c710f5 100644
--- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
+++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
@@ -19,20 +19,15 @@
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEvent;
-import java.util.ArrayList;
-import java.util.List;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
-import org.opencv.core.Point;
 import org.photonvision.common.dataflow.CVPipelineResultConsumer;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.networktables.NTTopicSet;
 import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
-import org.photonvision.targeting.TargetCorner;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
 
@@ -136,7 +131,9 @@ public void updateCameraNickname(String newCameraNickname) {
     public void accept(CVPipelineResult result) {
         var simplified =
                 new PhotonPipelineResult(
-                        result.getLatencyMillis(), simpleFromTrackedTargets(result.targets));
+                        result.getLatencyMillis(),
+                        TrackedTarget.simpleFromTrackedTargets(result.targets),
+                        result.multiTagResult);
         Packet packet = new Packet(simplified.getPacketSize());
         simplified.populatePacket(packet);
 
@@ -197,39 +194,4 @@ public void accept(CVPipelineResult result) {
         // TODO...nt4... is this needed?
         rootTable.getInstance().flush();
     }
-
-    public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
-        var ret = new ArrayList<PhotonTrackedTarget>();
-        for (var t : targets) {
-            var minAreaRectCorners = new ArrayList<TargetCorner>();
-            var detectedCorners = new ArrayList<TargetCorner>();
-            {
-                var points = new Point[4];
-                t.getMinAreaRect().points(points);
-                for (int i = 0; i < 4; i++) {
-                    minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y));
-                }
-            }
-            {
-                var points = t.getTargetCorners();
-                for (Point point : points) {
-                    detectedCorners.add(new TargetCorner(point.x, point.y));
-                }
-            }
-
-            ret.add(
-                    new PhotonTrackedTarget(
-                            t.getYaw(),
-                            t.getPitch(),
-                            t.getArea(),
-                            t.getSkew(),
-                            t.getFiducialId(),
-                            t.getBestCameraToTarget3d(),
-                            t.getAltCameraToTarget3d(),
-                            t.getPoseAmbiguity(),
-                            minAreaRectCorners,
-                            detectedCorners));
-        }
-        return ret;
-    }
 }
diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
index 8abb97866b..a62e0f3b4c 100644
--- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
+++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
@@ -17,9 +17,14 @@
 
 package org.photonvision.common.dataflow.networktables;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEvent;
+import edu.wpi.first.networktables.NetworkTableEvent.Kind;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StringSubscriber;
+import java.io.IOException;
+import java.util.EnumSet;
 import java.util.HashMap;
 import java.util.function.Consumer;
 import org.photonvision.PhotonVersion;
@@ -32,17 +37,33 @@
 import org.photonvision.common.scripting.ScriptEventType;
 import org.photonvision.common.scripting.ScriptManager;
 import org.photonvision.common.util.TimedTaskManager;
+import org.photonvision.common.util.file.JacksonUtils;
 
 public class NetworkTablesManager {
     private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
     private final String kRootTableName = "/photonvision";
+    private final String kFieldLayoutName = "apriltag_field_layout";
     public final NetworkTable kRootTable = ntInstance.getTable(kRootTableName);
 
-    private boolean isRetryingConnection = false;
+    private final NTLogger m_ntLogger = new NTLogger();
+
+    private boolean m_isRetryingConnection = false;
+
+    private StringSubscriber m_fieldLayoutSubscriber =
+            kRootTable.getStringTopic(kFieldLayoutName).subscribe("");
 
     private NetworkTablesManager() {
-        ntInstance.addLogger(0, 255, new NTLogger()); // to hide error messages
+        ntInstance.addLogger(255, 255, (event) -> {}); // to hide error messages
+        ntInstance.addConnectionListener(true, m_ntLogger); // to hide error messages
+
+        ntInstance.addListener(
+                m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged);
+
         TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
+
+        // Get the UI state in sync with the backend. NT should fire a callback when it first connects
+        // to the robot
+        broadcastConnectedStatus();
     }
 
     private static NetworkTablesManager INSTANCE;
@@ -56,24 +77,33 @@ public static NetworkTablesManager getInstance() {
 
     private static class NTLogger implements Consumer<NetworkTableEvent> {
         private boolean hasReportedConnectionFailure = false;
-        private long lastConnectMessageMillis = 0;
 
         @Override
         public void accept(NetworkTableEvent event) {
-            if (!hasReportedConnectionFailure && event.logMessage.message.contains("timed out")) {
-                logger.error("NT Connection has failed! Will retry in background.");
+            var isConnEvent = event.is(Kind.kConnected);
+            var isDisconnEvent = event.is(Kind.kDisconnected);
+
+            if (!hasReportedConnectionFailure && isDisconnEvent) {
+                var msg =
+                        String.format(
+                                "NT lost connection to %s:%d! (NT version %d). Will retry in background.",
+                                event.connInfo.remote_ip,
+                                event.connInfo.remote_port,
+                                event.connInfo.protocol_version);
+                logger.error(msg);
+
                 hasReportedConnectionFailure = true;
                 getInstance().broadcastConnectedStatus();
-            } else if (event.logMessage.message.contains("connected")
-                    && System.currentTimeMillis() - lastConnectMessageMillis > 125) {
-                String connectionDescription = "(unknown)";
-                var connections = getInstance().ntInstance.getConnections();
-                if (connections.length > 0) {
-                    connectionDescription = connections[0].remote_ip + ":" + connections[0].remote_port;
-                }
-                logger.info("NT Connected to " + connectionDescription + "!");
+            } else if (isConnEvent && event.connInfo != null) {
+                var msg =
+                        String.format(
+                                "NT connected to %s:%d! (NT version %d)",
+                                event.connInfo.remote_ip,
+                                event.connInfo.remote_port,
+                                event.connInfo.protocol_version);
+                logger.info(msg);
+
                 hasReportedConnectionFailure = false;
-                lastConnectMessageMillis = System.currentTimeMillis();
                 ScriptManager.queueEvent(ScriptEventType.kNTConnected);
                 getInstance().broadcastVersion();
                 getInstance().broadcastConnectedStatus();
@@ -81,6 +111,23 @@ public void accept(NetworkTableEvent event) {
         }
     }
 
+    private void onFieldLayoutChanged(NetworkTableEvent event) {
+        var atfl_json = event.valueData.value.getString();
+        try {
+            System.out.println("Got new field layout!");
+            var atfl = JacksonUtils.deserialize(atfl_json, AprilTagFieldLayout.class);
+            ConfigManager.getInstance().getConfig().setApriltagFieldLayout(atfl);
+            ConfigManager.getInstance().requestSave();
+            DataChangeService.getInstance()
+                    .publishEvent(
+                            new OutgoingUIEvent<>(
+                                    "fullsettings", ConfigManager.getInstance().getConfig().toHashMap()));
+        } catch (IOException e) {
+            logger.error("Error deserializing atfl!");
+            logger.error(atfl_json);
+        }
+    }
+
     public void broadcastConnectedStatus() {
         TimedTaskManager.getInstance().addOneShotTask(this::broadcastConnectedStatusImpl, 1000L);
     }
@@ -122,10 +169,10 @@ private void setClientMode(String ntServerAddress) {
         ntInstance.startClient4("photonvision");
         try {
             int t = Integer.parseInt(ntServerAddress);
-            if (!isRetryingConnection) logger.info("Starting NT Client, server team is " + t);
+            if (!m_isRetryingConnection) logger.info("Starting NT Client, server team is " + t);
             ntInstance.setServerTeam(t);
         } catch (NumberFormatException e) {
-            if (!isRetryingConnection)
+            if (!m_isRetryingConnection)
                 logger.info("Starting NT Client, server IP is \"" + ntServerAddress + "\"");
             ntInstance.setServer(ntServerAddress);
         }
@@ -151,8 +198,8 @@ private void ntTick() {
             setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig());
         }
 
-        if (!ntInstance.isConnected() && !isRetryingConnection) {
-            isRetryingConnection = true;
+        if (!ntInstance.isConnected() && !m_isRetryingConnection) {
+            m_isRetryingConnection = true;
             logger.error(
                     "[NetworkTablesManager] Could not connect to the robot! Will retry in the background...");
         }
diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
index 0a9de04448..17ed240921 100644
--- a/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
+++ b/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
@@ -24,6 +24,7 @@
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.SerializationUtils;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 
 public class UIDataPublisher implements CVPipelineResultConsumer {
@@ -40,23 +41,30 @@ public UIDataPublisher(int index) {
     public void accept(CVPipelineResult result) {
         long now = System.currentTimeMillis();
 
-        var dataMap = new HashMap<String, Object>();
-        dataMap.put("latency", result.getLatencyMillis());
-
         // only update the UI at 15hz
         if (lastUIResultUpdateTime + 1000.0 / 10.0 > now) return;
 
-        var uiMap = new HashMap<Integer, HashMap<String, Object>>();
-
+        var dataMap = new HashMap<String, Object>();
         dataMap.put("fps", result.fps);
-
-        var targets = result.targets;
-
-        var uiTargets = new ArrayList<HashMap<String, Object>>();
-        for (var t : targets) {
+        dataMap.put("latency", result.getLatencyMillis());
+        var uiTargets = new ArrayList<HashMap<String, Object>>(result.targets.size());
+        for (var t : result.targets) {
             uiTargets.add(t.toHashMap());
         }
         dataMap.put("targets", uiTargets);
+
+        // Only send Multitag Results if they are present, similar to 3d pose
+        if (result.multiTagResult.estimatedPose.isPresent) {
+            var multitagData = new HashMap<String, Object>();
+            multitagData.put(
+                    "bestTransform",
+                    SerializationUtils.transformToHashMap(result.multiTagResult.estimatedPose.best));
+            multitagData.put("bestReprojectionError", result.multiTagResult.estimatedPose.bestReprojErr);
+            multitagData.put("fiducialIDsUsed", result.multiTagResult.fiducialIDsUsed);
+            dataMap.put("multitagResult", multitagData);
+        }
+
+        var uiMap = new HashMap<Integer, HashMap<String, Object>>();
         uiMap.put(index, dataMap);
 
         DataChangeService.getInstance()
diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java
index 008724e9b7..2a049cccae 100644
--- a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java
+++ b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java
@@ -138,6 +138,8 @@ public void setBrightnessPercent(int percent) {
     private void onJvmExit() {
         logger.info("Shutting down LEDs...");
         if (visionLED != null) visionLED.setState(false);
+
+        logger.info("Force-flushing settings...");
         ConfigManager.getInstance().saveToDisk();
     }
 
diff --git a/photon-core/src/main/java/org/photonvision/common/networking/RoborioFinder.java b/photon-core/src/main/java/org/photonvision/common/networking/RoborioFinder.java
index e6b42112fc..605480e296 100644
--- a/photon-core/src/main/java/org/photonvision/common/networking/RoborioFinder.java
+++ b/photon-core/src/main/java/org/photonvision/common/networking/RoborioFinder.java
@@ -45,7 +45,7 @@ public void findRios() {
         // Separate from the above so we don't hold stuff up
         System.setProperty("java.net.preferIPv4Stack", "true");
         subMap.put(
-                "deviceips",
+                "deviceIps",
                 Arrays.stream(CameraServerJNI.getNetworkInterfaces())
                         .filter(it -> !it.equals("0.0.0.0"))
                         .toArray());
diff --git a/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java b/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java
index a211b545ed..6fc3ea9c30 100644
--- a/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java
+++ b/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java
@@ -17,6 +17,7 @@
 
 package org.photonvision.common.util;
 
+import edu.wpi.first.math.geometry.Transform3d;
 import java.util.HashMap;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
@@ -43,4 +44,18 @@ public static HashMap<String, Object> objectToHashMap(Object src) {
         }
         return ret;
     }
+
+    public static HashMap<String, Object> transformToHashMap(Transform3d transform) {
+        var ret = new HashMap<String, Object>();
+        ret.put("x", transform.getTranslation().getX());
+        ret.put("y", transform.getTranslation().getY());
+        ret.put("z", transform.getTranslation().getZ());
+        ret.put("qw", transform.getRotation().getQuaternion().getW());
+        ret.put("qx", transform.getRotation().getQuaternion().getX());
+        ret.put("qy", transform.getRotation().getQuaternion().getY());
+        ret.put("qz", transform.getRotation().getQuaternion().getZ());
+
+        ret.put("angle_z", transform.getRotation().getZ());
+        return ret;
+    }
 }
diff --git a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
index 526311afc5..2b6bb31ae3 100644
--- a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
+++ b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
@@ -17,10 +17,12 @@
 
 package org.photonvision.common.util.math;
 
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Nat;
+import edu.wpi.first.apriltag.AprilTagPoseEstimate;
 import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.geometry.CoordinateSystem;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.Arrays;
@@ -137,73 +139,46 @@ public static double lerp(double startValue, double endValue, double t) {
         return startValue + (endValue - startValue) * t;
     }
 
-    public static Pose3d EDNtoNWU(final Pose3d pose) {
-        // Change of basis matrix from EDN to NWU. Each column vector is one of the
-        // old basis vectors mapped to its representation in the new basis.
-        //
-        // E (+X) -> N (-Y), D (+Y) -> W (-Z), N (+Z) -> U (+X)
-        var R = new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0);
-
-        // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
-        double w = Math.sqrt(1.0 + R.get(0, 0) + R.get(1, 1) + R.get(2, 2)) / 2.0;
-        double x = (R.get(2, 1) - R.get(1, 2)) / (4.0 * w);
-        double y = (R.get(0, 2) - R.get(2, 0)) / (4.0 * w);
-        double z = (R.get(1, 0) - R.get(0, 1)) / (4.0 * w);
-        var rotationQuat = new Rotation3d(new Quaternion(w, x, y, z));
-
-        return new Pose3d(
-                pose.getTranslation().rotateBy(rotationQuat), pose.getRotation().rotateBy(rotationQuat));
-    }
-
     /**
-     * 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
+     * OpenCV uses the EDN coordinate system, but WPILib uses NWU. Converts a camera-to-target
+     * transformation from EDN to NWU.
+     *
+     * <p>Note: The detected target's rvec and tvec perform a rotation-translation transformation
+     * which converts points in the target's coordinate system to the camera's. This means applying
+     * the transformation to the target point (0,0,0) for example would give the target's center
+     * relative to the camera. Conveniently, if we make a translation-rotation transformation out of
+     * these components instead, we get the transformation from the camera to the target.
+     *
+     * @param cameraToTarget3d A camera-to-target Transform3d in EDN.
+     * @return A camera-to-target Transform3d in NWU.
      */
-    private static final Rotation3d WPILIB_BASE_ROTATION =
-            new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 1, 0, 0, 0, 1, 1, 0, 0));
-
     public static Transform3d convertOpenCVtoPhotonTransform(Transform3d cameraToTarget3d) {
         // TODO: Refactor into new pipe?
-        // CameraToTarget _should_ be in opencv-land EDN
-        var nwu =
-                CoordinateSystem.convert(
-                        new Pose3d().transformBy(cameraToTarget3d),
-                        CoordinateSystem.EDN(),
-                        CoordinateSystem.NWU());
-        return new Transform3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
-    }
-
-    public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
-        // TODO: Refactor into new pipe?
-        // CameraToTarget _should_ be in opencv-land EDN
-        var nwu =
-                CoordinateSystem.convert(
-                        new Pose3d().transformBy(cameraToTarget3d),
-                        CoordinateSystem.EDN(),
-                        CoordinateSystem.NWU());
-        return new Pose3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
+        return CoordinateSystem.convert(
+                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
+     * From the AprilTag repo:
+     * "The coordinate system has the origin at the camera center. The z-axis points from the camera
+     * center out the camera lens. The x-axis is to the right in the image taken by the camera, and
+     * y is down. The tag's coordinate frame is centered at the center of the tag, with x-axis to the
+     * right, y-axis down, and z-axis into the tag."
+     *
+     * This means our detected transformation will be in EDN. Our subsequent uses of the transformation,
+     * however, assume the tag's z-axis point away from the tag instead of into it. This means we
+     * have to correct the transformation's rotation.
      */
     private static final Rotation3d APRILTAG_BASE_ROTATION =
-            new Rotation3d(VecBuilder.fill(1, 0, 0), Units.degreesToRadians(180));
+            new Rotation3d(VecBuilder.fill(0, 1, 0), Units.degreesToRadians(180));
 
     /**
-     * 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.
+     * AprilTag returns a camera-to-tag transform in EDN, but the tag's z-axis points into the tag
+     * instead of away from it and towards the camera. This means we have to correct the
+     * transformation's rotation.
+     *
+     * @param pose The Transform3d with translation and rotation directly from the {@link
+     *     AprilTagPoseEstimate}.
      */
     public static Transform3d convertApriltagtoOpenCV(Transform3d pose) {
         var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation());
diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java
index fca4fdf261..15a6d0b9e0 100644
--- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java
+++ b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java
@@ -19,7 +19,10 @@
 
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
 import java.util.Arrays;
+import org.ejml.simple.SimpleMatrix;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfDouble;
@@ -32,7 +35,10 @@ public class JsonMat implements Releasable {
     public final int type;
     public final double[] data;
 
-    @JsonIgnore private Mat wrappedMat;
+    // Cached matrices to avoid object recreation
+    @JsonIgnore private Mat wrappedMat = null;
+    @JsonIgnore private Matrix wpilibMat = null;
+
     private MatOfDouble wrappedMatOfDouble;
 
     public JsonMat(int rows, int cols, double[] data) {
@@ -103,6 +109,14 @@ public MatOfDouble getAsMatOfDouble() {
         return this.wrappedMatOfDouble;
     }
 
+    @JsonIgnore
+    public <R extends Num, C extends Num> Matrix<R, C> getAsWpilibMat() {
+        if (wpilibMat == null) {
+            wpilibMat = new Matrix<R, C>(new SimpleMatrix(rows, cols, true, data));
+        }
+        return (Matrix<R, C>) wpilibMat;
+    }
+
     @Override
     public void release() {
         getAsMat().release();
diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java
index f03db3a60e..cedda04d22 100644
--- a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java
+++ b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java
@@ -39,7 +39,7 @@ public class FrameStaticProperties {
      * Instantiates a new Frame static properties.
      *
      * @param mode The Video Mode of the camera.
-     * @param fov The fov of the image.
+     * @param fov The FOV (Field Of Vision) of the image in degrees.
      */
     public FrameStaticProperties(VideoMode mode, double fov, CameraCalibrationCoefficients cal) {
         this(mode != null ? mode.width : 1, mode != null ? mode.height : 1, fov, cal);
@@ -48,9 +48,9 @@ public FrameStaticProperties(VideoMode mode, double fov, CameraCalibrationCoeffi
     /**
      * Instantiates a new Frame static properties.
      *
-     * @param imageWidth The width of the image.
-     * @param imageHeight The width of the image.
-     * @param fov The fov of the image.
+     * @param imageWidth The width of the image in pixels.
+     * @param imageHeight The width of the image in pixels.
+     * @param fov The FOV (Field Of Vision) of the image in degrees.
      */
     public FrameStaticProperties(
             int imageWidth, int imageHeight, double fov, CameraCalibrationCoefficients cal) {
@@ -61,30 +61,47 @@ public FrameStaticProperties(
 
         imageArea = this.imageWidth * this.imageHeight;
 
-        // Todo -- if we have calibration, use it's center point?
-        centerX = ((double) this.imageWidth / 2) - 0.5;
-        centerY = ((double) this.imageHeight / 2) - 0.5;
-        centerPoint = new Point(centerX, centerY);
-
-        // TODO if we have calibration use it here instead
         // pinhole model calculations
-        DoubleCouple horizVertViews =
-                calculateHorizontalVerticalFoV(this.fov, this.imageWidth, this.imageHeight);
+        if (cameraCalibration != null && cameraCalibration.getCameraIntrinsicsMat() != null) {
+            // Use calibration data
+            var camIntrinsics = cameraCalibration.getCameraIntrinsicsMat();
+            centerX = camIntrinsics.get(0, 2)[0];
+            centerY = camIntrinsics.get(1, 2)[0];
+            centerPoint = new Point(centerX, centerY);
+            horizontalFocalLength = camIntrinsics.get(0, 0)[0];
+            verticalFocalLength = camIntrinsics.get(1, 1)[0];
+        } else {
+            // No calibration data. Calculate from user provided diagonal FOV
+            centerX = (this.imageWidth / 2.0) - 0.5;
+            centerY = (this.imageHeight / 2.0) - 0.5;
+            centerPoint = new Point(centerX, centerY);
 
-        horizontalFocalLength = this.imageWidth / (2 * Math.tan(horizVertViews.getFirst() / 2));
-        verticalFocalLength = this.imageHeight / (2 * Math.tan(horizVertViews.getSecond() / 2));
+            DoubleCouple horizVertViews =
+                    calculateHorizontalVerticalFoV(this.fov, this.imageWidth, this.imageHeight);
+            double horizFOV = Math.toRadians(horizVertViews.getFirst());
+            double vertFOV = Math.toRadians(horizVertViews.getSecond());
+            horizontalFocalLength = (this.imageWidth / 2.0) / Math.tan(horizFOV / 2.0);
+            verticalFocalLength = (this.imageHeight / 2.0) / Math.tan(vertFOV / 2.0);
+        }
     }
 
+    /**
+     * Calculates the horizontal and vertical FOV components from a given diagonal FOV and image size.
+     *
+     * @param diagonalFoV Diagonal FOV in degrees
+     * @param imageWidth Image width in pixels
+     * @param imageHeight Image height in pixels
+     * @return Horizontal and vertical FOV in degrees
+     */
     public static DoubleCouple calculateHorizontalVerticalFoV(
             double diagonalFoV, int imageWidth, int imageHeight) {
-        double diagonalView = Math.toRadians(diagonalFoV);
+        diagonalFoV = Math.toRadians(diagonalFoV);
         double diagonalAspect = Math.hypot(imageWidth, imageHeight);
 
         double horizontalView =
-                Math.atan(Math.tan(diagonalView / 2) * (imageWidth / diagonalAspect)) * 2;
-        double verticalView =
-                Math.atan(Math.tan(diagonalView / 2) * (imageHeight / diagonalAspect)) * 2;
+                Math.atan(Math.tan(diagonalFoV / 2) * (imageWidth / diagonalAspect)) * 2;
+        double verticalView = Math.atan(Math.tan(diagonalFoV / 2) * (imageHeight / diagonalAspect)) * 2;
 
-        return new DoubleCouple(horizontalView, verticalView);
+        return new DoubleCouple(Math.toDegrees(horizontalView), Math.toDegrees(verticalView));
     }
 }
diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java
index 2552b5fbdb..57bc312b2f 100644
--- a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java
+++ b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java
@@ -32,58 +32,57 @@
 import org.photonvision.vision.opencv.CVMat;
 
 public class FileSaveFrameConsumer implements Consumer<CVMat> {
+    private final Logger logger = new Logger(FileSaveFrameConsumer.class, LogGroup.General);
+
     // Formatters to generate unique, timestamped file names
     private static final String FILE_PATH = ConfigManager.getInstance().getImageSavePath().toString();
     private static final String FILE_EXTENSION = ".jpg";
+    private static final String NT_SUFFIX = "SaveImgCmd";
+
     DateFormat df = new SimpleDateFormat("yyyy-MM-dd");
     DateFormat tf = new SimpleDateFormat("hhmmssSS");
-    private final String NT_SUFFIX = "SaveImgCmd";
-    private final String ntEntryName;
-    private NetworkTable subTable;
+
     private final NetworkTable rootTable;
-    private final Logger logger;
-    private long imgSaveCountInternal = 0;
-    private String camNickname;
-    private final String fnamePrefix;
-    private IntegerEntry entry;
+    private NetworkTable subTable;
+    private final String ntEntryName;
+    private IntegerEntry saveFrameEntry;
+
+    private String cameraNickname;
+    private final String streamType;
+
+    private long savedImagesCount = 0;
 
     public FileSaveFrameConsumer(String camNickname, String streamPrefix) {
-        this.fnamePrefix = camNickname + "_" + streamPrefix;
         this.ntEntryName = streamPrefix + NT_SUFFIX;
+        this.cameraNickname = camNickname;
+        this.streamType = streamPrefix;
+
         this.rootTable = NetworkTablesManager.getInstance().kRootTable;
         updateCameraNickname(camNickname);
-        this.logger = new Logger(FileSaveFrameConsumer.class, this.camNickname, LogGroup.General);
     }
 
     public void accept(CVMat image) {
         if (image != null && image.getMat() != null && !image.getMat().empty()) {
-            var curCommand = entry.get(); // default to just our current count
-            if (curCommand >= 0) {
-                // Only do something if we got a valid current command
-                if (imgSaveCountInternal < curCommand) {
-                    // Save one frame.
-                    // Create the filename
-                    Date now = new Date();
-                    String savefile =
-                            FILE_PATH
-                                    + File.separator
-                                    + fnamePrefix
-                                    + "_"
-                                    + df.format(now)
-                                    + "T"
-                                    + tf.format(now)
-                                    + FILE_EXTENSION;
-
-                    // write to file
-                    Imgcodecs.imwrite(savefile, image.getMat());
-
-                    // Count one more image saved
-                    imgSaveCountInternal++;
-                    logger.info("Saved new image at " + savefile);
-
-                } else if (imgSaveCountInternal > curCommand) {
-                    imgSaveCountInternal = curCommand;
-                }
+            long currentCount = saveFrameEntry.get();
+
+            // Await save request
+            if (currentCount == -1) return;
+
+            // The requested count is greater than the actual count
+            if (savedImagesCount < currentCount) {
+                Date now = new Date();
+
+                String fileName =
+                        cameraNickname + "_" + streamType + "_" + df.format(now) + "T" + tf.format(now);
+                String saveFilePath = FILE_PATH + File.separator + fileName + FILE_EXTENSION;
+
+                Imgcodecs.imwrite(saveFilePath, image.getMat());
+
+                savedImagesCount++;
+                logger.info("Saved new image at " + saveFilePath);
+            } else if (savedImagesCount > currentCount) {
+                // Reset local value with NT value in case of de-sync
+                savedImagesCount = currentCount;
             }
         }
     }
@@ -97,9 +96,14 @@ public void updateCameraNickname(String newCameraNickname) {
         }
 
         // Recreate and re-init network tables structure
-        this.camNickname = newCameraNickname;
-        this.subTable = rootTable.getSubTable(this.camNickname);
-        this.subTable.getEntry(ntEntryName).setInteger(imgSaveCountInternal);
-        this.entry = subTable.getIntegerTopic(ntEntryName).getEntry(-1); // Default negative
+        this.cameraNickname = newCameraNickname;
+        this.subTable = rootTable.getSubTable(this.cameraNickname);
+        this.subTable.getEntry(ntEntryName).setInteger(savedImagesCount);
+        this.saveFrameEntry = subTable.getIntegerTopic(ntEntryName).getEntry(-1); // Default negative
+    }
+
+    public void overrideTakeSnapshot() {
+        // Simulate NT change
+        saveFrameEntry.set(saveFrameEntry.get() + 1);
     }
 }
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java
index 32ea2938b7..5c1497db3e 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java
@@ -27,8 +27,6 @@ public class AprilTagDetectionPipe
         extends CVPipe<CVMat, List<AprilTagDetection>, AprilTagDetectionPipeParams> {
     private final AprilTagDetector m_detector = new AprilTagDetector();
 
-    boolean useNativePoseEst;
-
     public AprilTagDetectionPipe() {
         super();
 
@@ -62,8 +60,4 @@ public void setParams(AprilTagDetectionPipeParams newParams) {
 
         super.setParams(newParams);
     }
-
-    public void setNativePoseEstimationEnabled(boolean enabled) {
-        this.useNativePoseEst = enabled;
-    }
 }
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java
index 9738202e3e..3c6563e182 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java
@@ -35,8 +35,6 @@ public class AprilTagPoseEstimatorPipe
     private final AprilTagPoseEstimator m_poseEstimator =
             new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0, 0, 0, 0, 0));
 
-    boolean useNativePoseEst;
-
     public AprilTagPoseEstimatorPipe() {
         super();
     }
@@ -94,10 +92,6 @@ public void setParams(AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams
         super.setParams(newParams);
     }
 
-    public void setNativePoseEstimationEnabled(boolean enabled) {
-        this.useNativePoseEst = enabled;
-    }
-
     public static class AprilTagPoseEstimatorPipeParams {
         final AprilTagPoseEstimator.Config config;
         final CameraCalibrationCoefficients calibration;
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java
index ff7f6fd3ea..bed3c6b5ef 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java
@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.geometry.Translation2d;
 import java.util.*;
 import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
@@ -25,6 +24,10 @@
 import org.photonvision.vision.pipe.CVPipe;
 import org.photonvision.vision.target.TrackedTarget;
 
+/**
+ * Determines the target corners of the {@link TrackedTarget}. The {@link
+ * CornerDetectionPipeParameters} affect how these corners are calculated.
+ */
 public class CornerDetectionPipe
         extends CVPipe<
                 List<TrackedTarget>,
@@ -49,15 +52,15 @@ protected List<TrackedTarget> process(List<TrackedTarget> targetList) {
     }
 
     /**
-     * @param target the target to find the corners of.
-     * @return the corners. left top, left bottom, right bottom, right top
+     * @param target The target to find the corners of.
+     * @return Corners: (bottom-left, bottom-right, top-right, top-left)
      */
     private List<Point> findBoundingBoxCorners(TrackedTarget target) {
         // extract the corners
         var points = new Point[4];
         target.m_mainContour.getMinAreaRect().points(points);
 
-        // find the tl/tr/bl/br corners
+        // find the bl/br/tr/tl corners
         // first, min by left/right
         var list_ = Arrays.asList(points);
         list_.sort(leftRightComparator);
@@ -68,13 +71,12 @@ private List<Point> findBoundingBoxCorners(TrackedTarget target) {
         var right = new ArrayList<>(List.of(list_.get(2), list_.get(3)));
         right.sort(verticalComparator);
 
-        // tl tr bl br
         var tl = left.get(0);
         var bl = left.get(1);
         var tr = right.get(0);
         var br = right.get(1);
 
-        return List.of(tl, bl, br, tr);
+        return List.of(bl, br, tr, tl);
     }
 
     /**
@@ -83,31 +85,22 @@ private List<Point> findBoundingBoxCorners(TrackedTarget target) {
      * @return The straight line distance between them.
      */
     private static double distanceBetween(Point a, Point b) {
-        return Math.sqrt(Math.pow(a.x - b.x, 2) + Math.pow(a.y - b.y, 2));
-    }
-
-    /**
-     * @param a First point.
-     * @param b Second point.
-     * @return The straight line distance between them.
-     */
-    private static double distanceBetween(Translation2d a, Translation2d b) {
-        return Math.sqrt(Math.pow(a.getX() - b.getX(), 2) + Math.pow(a.getY() - b.getY(), 2));
+        double xDelta = a.x - b.x;
+        double yDelta = a.y - b.y;
+        return Math.sqrt(xDelta * xDelta + yDelta * yDelta);
     }
 
     /**
-     * Find the 4 most extreme corners,
+     * Find the 4 most extreme corners of the target's contour.
      *
-     * @param target the target to track.
-     * @param convexHull weather to use the convex hull of the target.
-     * @return the 4 extreme corners of the contour.
+     * @param target The target to track.
+     * @param convexHull Whether to use the convex hull of the contour instead.
+     * @return The 4 extreme corners of the contour: (bottom-left, bottom-right, top-right, top-left)
      */
     private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) {
         var centroid = target.getMinAreaRect().center;
-        Comparator<Point> distanceProvider =
-                Comparator.comparingDouble(
-                        (Point point) ->
-                                Math.sqrt(Math.pow(centroid.x - point.x, 2) + Math.pow(centroid.y - point.y, 2)));
+        Comparator<Point> compareCenterDist =
+                Comparator.comparingDouble((Point point) -> distanceBetween(centroid, point));
 
         MatOfPoint2f targetContour;
         if (convexHull) {
@@ -141,17 +134,17 @@ private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boo
         // left top, left bottom, right bottom, right top
         var boundingBoxCorners = findBoundingBoxCorners(target);
 
-        var distanceToTlComparator =
-                Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)));
-
-        var distanceToTrComparator =
+        var compareDistToTl =
                 Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)));
 
-        // top left and top right are the poly corners closest to the bounding box tl and tr
-        pointList.sort(distanceToTlComparator);
+        var compareDistToTr =
+                Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(2)));
+
+        // top left and top right are the poly corners closest to the bouding box tl and tr
+        pointList.sort(compareDistToTl);
         var tl = pointList.get(0);
         pointList.remove(tl);
-        pointList.sort(distanceToTrComparator);
+        pointList.sort(compareDistToTr);
         var tr = pointList.get(0);
         pointList.remove(tr);
 
@@ -177,11 +170,11 @@ private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boo
                 }
         }
         if (leftList.isEmpty() || rightList.isEmpty()) return null;
-        leftList.sort(distanceProvider);
-        rightList.sort(distanceProvider);
+        leftList.sort(compareCenterDist);
+        rightList.sort(compareCenterDist);
         var bl = leftList.get(leftList.size() - 1);
         var br = rightList.get(rightList.size() - 1);
-        return List.of(tl, bl, br, tr);
+        return List.of(bl, br, tr, tl);
     }
 
     public static class CornerDetectionPipeParameters {
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java
index b88bd0ba17..99927ea0f1 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java
@@ -127,7 +127,7 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {
 
                 // Draw X, Y and Z axis
                 MatOfPoint3f pointMat = new MatOfPoint3f();
-                // Those points are in opencv-land, but we are in NWU
+                // OpenCV expects coordinates in EDN, but we want to visualize in NWU
                 // NWU | EDN
                 // X: Z
                 // Y: -X
@@ -136,11 +136,15 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {
                 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));
+                                new Point3(0, 0, AXIS_LEN), // x-axis
+                                new Point3(-AXIS_LEN, 0, 0), // y-axis
+                                new Point3(0, -AXIS_LEN, 0)); // z-axis
                 pointMat.fromList(list);
 
+                // The detected target's rvec and tvec perform a rotation-translation transformation which
+                // converts points in the target's coordinate system to the camera's. This means applying
+                // the transformation to the target point (0,0,0) for example would give the target's center
+                // relative to the camera.
                 Calib3d.projectPoints(
                         pointMat,
                         target.getCameraRelativeRvec(),
@@ -152,19 +156,22 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {
                 var axisPoints = tempMat.toList();
                 dividePointList(axisPoints);
 
-                // Red = x, green y, blue z
+                // XYZ is RGB
+                // y-axis = green
                 Imgproc.line(
                         in.getLeft(),
                         axisPoints.get(0),
                         axisPoints.get(2),
                         ColorHelper.colorToScalar(Color.GREEN),
                         3);
+                // z-axis = blue
                 Imgproc.line(
                         in.getLeft(),
                         axisPoints.get(0),
                         axisPoints.get(3),
                         ColorHelper.colorToScalar(Color.BLUE),
                         3);
+                // x-axis = red
                 Imgproc.line(
                         in.getLeft(),
                         axisPoints.get(0),
@@ -172,6 +179,7 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {
                         ColorHelper.colorToScalar(Color.RED),
                         3);
 
+                // box edges perpendicular to tag
                 for (int i = 0; i < bottomPoints.size(); i++) {
                     Imgproc.line(
                             in.getLeft(),
@@ -180,6 +188,7 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {
                             ColorHelper.colorToScalar(Color.blue),
                             3);
                 }
+                // box edges parallel to tag
                 for (int i = 0; i < topPoints.size(); i++) {
                     Imgproc.line(
                             in.getLeft(),
@@ -265,18 +274,6 @@ private void divideMat2f(MatOfPoint2f src, MatOfPoint dst) {
         dst.fromArray(pointArray);
     }
 
-    private void divideMat2f(MatOfPoint2f src, MatOfPoint2f dst) {
-        var hull = src.toArray();
-        var pointArray = new Point[hull.length];
-        for (int i = 0; i < hull.length; i++) {
-            var hullAtI = hull[i];
-            pointArray[i] =
-                    new Point(
-                            hullAtI.x / (double) params.divisor.value, hullAtI.y / (double) params.divisor.value);
-        }
-        dst.fromArray(pointArray);
-    }
-
     /** Scale a given point list by the current frame divisor. the point list is mutated! */
     private void dividePointList(List<Point> points) {
         for (var p : points) {
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java
new file mode 100644
index 0000000000..5ce9f124cc
--- /dev/null
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java
@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import java.util.ArrayList;
+import java.util.List;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.estimation.VisionEstimation;
+import org.photonvision.targeting.MultiTargetPNPResults;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+import org.photonvision.vision.pipe.CVPipe;
+import org.photonvision.vision.target.TrackedTarget;
+
+/** Estimate the camera pose given multiple Apriltag observations */
+public class MultiTargetPNPPipe
+        extends CVPipe<
+                List<TrackedTarget>, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
+    private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule);
+
+    private boolean hasWarned = false;
+
+    @Override
+    protected MultiTargetPNPResults process(List<TrackedTarget> targetList) {
+        if (params.cameraCoefficients == null
+                || params.cameraCoefficients.getCameraIntrinsicsMat() == null
+                || params.cameraCoefficients.getDistCoeffsMat() == null) {
+            if (!hasWarned) {
+                logger.warn(
+                        "Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution...");
+                hasWarned = true;
+            }
+            return new MultiTargetPNPResults();
+        }
+
+        return calculateCameraInField(targetList);
+    }
+
+    private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetList) {
+        // Find tag IDs that exist in the tag layout
+        var tagIDsUsed = new ArrayList<Integer>();
+        for (var target : targetList) {
+            int id = target.getFiducialId();
+            if (params.atfl.getTagPose(id).isPresent()) tagIDsUsed.add(id);
+        }
+
+        // Only run with multiple targets
+        if (tagIDsUsed.size() < 2) {
+            return new MultiTargetPNPResults();
+        }
+
+        var estimatedPose =
+                VisionEstimation.estimateCamPosePNP(
+                        params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(),
+                        params.cameraCoefficients.distCoeffs.getAsWpilibMat(),
+                        TrackedTarget.simpleFromTrackedTargets(targetList),
+                        params.atfl);
+
+        return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);
+    }
+
+    public static class MultiTargetPNPPipeParams {
+        private final CameraCalibrationCoefficients cameraCoefficients;
+        private final AprilTagFieldLayout atfl;
+
+        public MultiTargetPNPPipeParams(
+                CameraCalibrationCoefficients cameraCoefficients, AprilTagFieldLayout atfl) {
+            this.cameraCoefficients = cameraCoefficients;
+            this.atfl = atfl;
+        }
+    }
+}
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java
index db773f6375..6a887d3a18 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java
@@ -18,7 +18,6 @@
 package org.photonvision.vision.pipe.impl;
 
 import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -27,7 +26,6 @@
 import org.opencv.core.Core;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfPoint2f;
-import org.opencv.core.Scalar;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.math.MathUtils;
@@ -99,33 +97,12 @@ private void calculateTargetPose(TrackedTarget target) {
                         VecBuilder.fill(rVec.get(0, 0)[0], rVec.get(1, 0)[0], rVec.get(2, 0)[0]),
                         Core.norm(rVec));
 
-        Pose3d targetPose = MathUtils.convertOpenCVtoPhotonPose(new Transform3d(translation, rotation));
-        target.setBestCameraToTarget3d(
-                new Transform3d(targetPose.getTranslation(), targetPose.getRotation()));
+        Transform3d camToTarget =
+                MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation));
+        target.setBestCameraToTarget3d(camToTarget);
         target.setAltCameraToTarget3d(new Transform3d());
     }
 
-    Mat rotationMatrix = new Mat();
-    Mat inverseRotationMatrix = new Mat();
-    Mat pzeroWorld = new Mat();
-    Mat kMat = new Mat();
-    Mat scaledTvec;
-
-    /**
-     * Element-wise scale a matrix by a given factor
-     *
-     * @param src the source matrix
-     * @param factor by how much to scale each element
-     * @return the scaled matrix
-     */
-    @SuppressWarnings("SameParameterValue")
-    private static Mat matScale(Mat src, double factor) {
-        Mat dst = new Mat(src.rows(), src.cols(), src.type());
-        Scalar s = new Scalar(factor);
-        Core.multiply(src, s, dst);
-        return dst;
-    }
-
     public static class SolvePNPPipeParams {
         private final CameraCalibrationCoefficients cameraCoefficients;
         private final TargetModel targetModel;
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
index 41459a4c3f..290507fe7c 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
@@ -21,11 +21,16 @@
 import edu.wpi.first.apriltag.AprilTagDetector;
 import edu.wpi.first.apriltag.AprilTagPoseEstimate;
 import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config;
+import edu.wpi.first.math.geometry.CoordinateSystem;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import java.util.ArrayList;
 import java.util.List;
+import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
@@ -34,13 +39,17 @@
 import org.photonvision.vision.pipe.impl.AprilTagPoseEstimatorPipe;
 import org.photonvision.vision.pipe.impl.AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams;
 import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
+import org.photonvision.vision.pipe.impl.MultiTargetPNPPipe;
+import org.photonvision.vision.pipe.impl.MultiTargetPNPPipe.MultiTargetPNPPipeParams;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
 import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
 
 public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipelineSettings> {
     private final AprilTagDetectionPipe aprilTagDetectionPipe = new AprilTagDetectionPipe();
-    private final AprilTagPoseEstimatorPipe poseEstimatorPipe = new AprilTagPoseEstimatorPipe();
+    private final AprilTagPoseEstimatorPipe singleTagPoseEstimatorPipe =
+            new AprilTagPoseEstimatorPipe();
+    private final MultiTargetPNPPipe multiTagPNPPipe = new MultiTargetPNPPipe();
     private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
 
     private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.GREYSCALE;
@@ -91,17 +100,22 @@ protected void setPipeParamsImpl() {
 
         if (frameStaticProperties.cameraCalibration != null) {
             var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
-            if (cameraMatrix != null) {
+            if (cameraMatrix != null && cameraMatrix.rows() > 0) {
                 var cx = cameraMatrix.get(0, 2)[0];
                 var cy = cameraMatrix.get(1, 2)[0];
                 var fx = cameraMatrix.get(0, 0)[0];
                 var fy = cameraMatrix.get(1, 1)[0];
 
-                poseEstimatorPipe.setParams(
+                singleTagPoseEstimatorPipe.setParams(
                         new AprilTagPoseEstimatorPipeParams(
                                 new Config(tagWidth, fx, fy, cx, cy),
                                 frameStaticProperties.cameraCalibration,
                                 settings.numIterations));
+
+                // TODO global state ew
+                var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
+                multiTagPNPPipe.setParams(
+                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl));
             }
         }
     }
@@ -110,11 +124,6 @@ protected void setPipeParamsImpl() {
     protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings settings) {
         long sumPipeNanosElapsed = 0L;
 
-        List<TrackedTarget> targetList;
-
-        // Use the solvePNP Enabled flag to enable native pose estimation
-        aprilTagDetectionPipe.setNativePoseEstimationEnabled(settings.solvePNPEnabled);
-
         if (frame.type != FrameThresholdType.GREYSCALE) {
             // We asked for a GREYSCALE frame, but didn't get one -- best we can do is give up
             return new CVPipelineResult(0, 0, List.of());
@@ -124,42 +133,109 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
         tagDetectionPipeResult = aprilTagDetectionPipe.run(frame.processedImage);
         sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
 
-        targetList = new ArrayList<>();
-        for (AprilTagDetection detection : tagDetectionPipeResult.output) {
+        List<AprilTagDetection> detections = tagDetectionPipeResult.output;
+        List<AprilTagDetection> usedDetections = new ArrayList<>();
+        List<TrackedTarget> targetList = new ArrayList<>();
+
+        // Filter out detections based on pipeline settings
+        for (AprilTagDetection detection : detections) {
             // 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;
 
-            AprilTagPoseEstimate tagPoseEstimate = null;
-            if (settings.solvePNPEnabled) {
-                var poseResult = poseEstimatorPipe.run(detection);
-                sumPipeNanosElapsed += poseResult.nanosElapsed;
-                tagPoseEstimate = poseResult.output;
-            }
+            usedDetections.add(detection);
 
-            // populate the target list
-            // Challenge here is that TrackedTarget functions with OpenCV Contour
+            // Populate target list for multitag
+            // (TODO: Address circular dependencies. Multitag only requires corners and IDs, this should
+            // not be necessary.)
             TrackedTarget target =
                     new TrackedTarget(
                             detection,
-                            tagPoseEstimate,
+                            null,
                             new TargetCalculationParameters(
                                     false, null, null, null, null, frameStaticProperties));
 
-            var correctedBestPose = MathUtils.convertOpenCVtoPhotonPose(target.getBestCameraToTarget3d());
-            var correctedAltPose = MathUtils.convertOpenCVtoPhotonPose(target.getAltCameraToTarget3d());
+            targetList.add(target);
+        }
 
-            target.setBestCameraToTarget3d(
-                    new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
-            target.setAltCameraToTarget3d(
-                    new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
+        // Do multi-tag pose estimation
+        MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
+        if (settings.solvePNPEnabled && settings.doMultiTarget) {
+            var multiTagOutput = multiTagPNPPipe.run(targetList);
+            sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
+            multiTagResult = multiTagOutput.output;
+        }
 
-            targetList.add(target);
+        // Do single-tag pose estimation
+        if (settings.solvePNPEnabled) {
+            // Clear target list that was used for multitag so we can add target transforms
+            targetList.clear();
+            // TODO global state again ew
+            var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
+
+            for (AprilTagDetection detection : usedDetections) {
+                AprilTagPoseEstimate tagPoseEstimate = null;
+                // Do single-tag estimation when "always enabled" or if a tag was not used for multitag
+                if (settings.doSingleTargetAlways
+                        || !multiTagResult.fiducialIDsUsed.contains(Integer.valueOf(detection.getId()))) {
+                    var poseResult = singleTagPoseEstimatorPipe.run(detection);
+                    sumPipeNanosElapsed += poseResult.nanosElapsed;
+                    tagPoseEstimate = poseResult.output;
+                }
+
+                // If single-tag estimation was not done, this is a multi-target tag from the layout
+                if (tagPoseEstimate == null) {
+                    // compute this tag's camera-to-tag transform using the multitag result
+                    var tagPose = atfl.getTagPose(detection.getId());
+                    if (tagPose.isPresent()) {
+                        var camToTag =
+                                new Transform3d(
+                                        new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
+                        // match expected AprilTag coordinate system
+                        // TODO cleanup coordinate systems in wpilib 2024
+                        var apriltagTrl =
+                                CoordinateSystem.convert(
+                                        camToTag.getTranslation(), CoordinateSystem.NWU(), CoordinateSystem.EDN());
+                        var apriltagRot =
+                                CoordinateSystem.convert(
+                                                new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU())
+                                        .plus(
+                                                CoordinateSystem.convert(
+                                                        camToTag.getRotation(),
+                                                        CoordinateSystem.NWU(),
+                                                        CoordinateSystem.EDN()));
+                        apriltagRot = new Rotation3d(0, Math.PI, 0).plus(apriltagRot);
+                        camToTag = new Transform3d(apriltagTrl, apriltagRot);
+                        tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0);
+                    }
+                }
+
+                // populate the target list
+                // Challenge here is that TrackedTarget functions with OpenCV Contour
+                TrackedTarget target =
+                        new TrackedTarget(
+                                detection,
+                                tagPoseEstimate,
+                                new TargetCalculationParameters(
+                                        false, null, null, null, null, frameStaticProperties));
+
+                var correctedBestPose =
+                        MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d());
+                var correctedAltPose =
+                        MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d());
+
+                target.setBestCameraToTarget3d(
+                        new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
+                target.setAltCameraToTarget3d(
+                        new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
+
+                targetList.add(target);
+            }
         }
 
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
-        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, frame);
+        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
     }
 }
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
index 4821c7b759..47faf82e29 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
@@ -18,7 +18,6 @@
 package org.photonvision.vision.pipeline;
 
 import com.fasterxml.jackson.annotation.JsonTypeName;
-import java.util.Objects;
 import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.target.TargetModel;
 
@@ -33,6 +32,8 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
     public int numIterations = 40;
     public int hammingDist = 0;
     public int decisionMargin = 35;
+    public boolean doMultiTarget = true;
+    public boolean doSingleTargetAlways = false;
 
     // 3d settings
 
@@ -47,16 +48,42 @@ public AprilTagPipelineSettings() {
     }
 
     @Override
-    public boolean equals(Object o) {
-        if (this == o) return true;
-        if (o == null || getClass() != o.getClass()) return false;
-        if (!super.equals(o)) return false;
-        AprilTagPipelineSettings that = (AprilTagPipelineSettings) o;
-        return Objects.equals(tagFamily, that.tagFamily)
-                && Double.compare(decimate, that.decimate) == 0
-                && Double.compare(blur, that.blur) == 0
-                && threads == that.threads
-                && debug == that.debug
-                && refineEdges == that.refineEdges;
+    public int hashCode() {
+        final int prime = 31;
+        int result = super.hashCode();
+        result = prime * result + ((tagFamily == null) ? 0 : tagFamily.hashCode());
+        result = prime * result + decimate;
+        long temp;
+        temp = Double.doubleToLongBits(blur);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        result = prime * result + threads;
+        result = prime * result + (debug ? 1231 : 1237);
+        result = prime * result + (refineEdges ? 1231 : 1237);
+        result = prime * result + numIterations;
+        result = prime * result + hammingDist;
+        result = prime * result + decisionMargin;
+        result = prime * result + (doMultiTarget ? 1231 : 1237);
+        result = prime * result + (doSingleTargetAlways ? 1231 : 1237);
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (!super.equals(obj)) return false;
+        if (getClass() != obj.getClass()) return false;
+        AprilTagPipelineSettings other = (AprilTagPipelineSettings) obj;
+        if (tagFamily != other.tagFamily) return false;
+        if (decimate != other.decimate) return false;
+        if (Double.doubleToLongBits(blur) != Double.doubleToLongBits(other.blur)) return false;
+        if (threads != other.threads) return false;
+        if (debug != other.debug) return false;
+        if (refineEdges != other.refineEdges) return false;
+        if (numIterations != other.numIterations) return false;
+        if (hammingDist != other.hammingDist) return false;
+        if (decisionMargin != other.decisionMargin) return false;
+        if (doMultiTarget != other.doMultiTarget) return false;
+        if (doSingleTargetAlways != other.doSingleTargetAlways) return false;
+        return true;
     }
 }
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
index 261b8fd522..95008e77f9 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
@@ -34,6 +34,7 @@
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.SerializationUtils;
 import org.photonvision.common.util.file.FileUtils;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
@@ -146,6 +147,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se
                 sumPipeNanosElapsed,
                 fps, // Unused but here in case
                 Collections.emptyList(),
+                new MultiTargetPNPResults(),
                 new Frame(
                         new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties));
     }
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
index aa9fd800ab..5993cc9531 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
@@ -20,6 +20,7 @@
 import java.util.Collections;
 import java.util.List;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.target.TrackedTarget;
@@ -30,18 +31,33 @@ public class CVPipelineResult implements Releasable {
     public final double fps;
     public final List<TrackedTarget> targets;
     public final Frame inputAndOutputFrame;
+    public MultiTargetPNPResults multiTagResult;
 
     public CVPipelineResult(
             double processingNanos, double fps, List<TrackedTarget> targets, Frame inputFrame) {
+        this(processingNanos, fps, targets, new MultiTargetPNPResults(), inputFrame);
+    }
+
+    public CVPipelineResult(
+            double processingNanos,
+            double fps,
+            List<TrackedTarget> targets,
+            MultiTargetPNPResults multiTagResults,
+            Frame inputFrame) {
         this.processingNanos = processingNanos;
         this.fps = fps;
         this.targets = targets != null ? targets : Collections.emptyList();
+        this.multiTagResult = multiTagResults;
 
         this.inputAndOutputFrame = inputFrame;
     }
 
-    public CVPipelineResult(double processingNanos, double fps, List<TrackedTarget> targets) {
-        this(processingNanos, fps, targets, null);
+    public CVPipelineResult(
+            double processingNanos,
+            double fps,
+            List<TrackedTarget> targets,
+            MultiTargetPNPResults multiTagResults) {
+        this(processingNanos, fps, targets, multiTagResults, null);
     }
 
     public boolean hasTargets() {
diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
index d246a45b52..75978739e7 100644
--- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
+++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
@@ -354,6 +354,14 @@ public void startCalibration(UICalibrationData data) {
         setPipeline(PipelineManager.CAL_3D_INDEX);
     }
 
+    public void saveInputSnapshot() {
+        inputFrameSaver.overrideTakeSnapshot();
+    }
+
+    public void saveOutputSnapshot() {
+        outputFrameSaver.overrideTakeSnapshot();
+    }
+
     public void takeCalibrationSnapshot() {
         pipelineManager.calibration3dPipeline.takeSnapshot();
     }
diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java
index e16e4eeedd..e824ce89dc 100644
--- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java
+++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java
@@ -107,6 +107,12 @@ public void onDataChangeEvent(DataChangeEvent<?> event) {
                         parentModule.startCalibration(data);
                         parentModule.saveAndBroadcastAll();
                         return;
+                    case "saveInputSnapshot":
+                        parentModule.saveInputSnapshot();
+                        return;
+                    case "saveOutputSnapshot":
+                        parentModule.saveOutputSnapshot();
+                        return;
                     case "takeCalSnapshot":
                         parentModule.takeCalibrationSnapshot();
                         return;
diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java
index 60b278aae4..5e64237443 100644
--- a/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java
+++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java
@@ -23,14 +23,29 @@
 import org.photonvision.vision.opencv.DualOffsetValues;
 
 public class TargetCalculations {
-    public static double calculateYaw(
-            double offsetCenterX, double targetCenterX, double horizontalFocalLength) {
-        return Math.toDegrees(Math.atan((offsetCenterX - targetCenterX) / horizontalFocalLength));
-    }
-
-    public static double calculatePitch(
-            double offsetCenterY, double targetCenterY, double verticalFocalLength) {
-        return -Math.toDegrees(Math.atan((offsetCenterY - targetCenterY) / verticalFocalLength));
+    /**
+     * Calculates the yaw and pitch of a point in the image. Yaw and pitch must be calculated together
+     * to account for perspective distortion. Yaw is positive right, and pitch is positive up.
+     *
+     * @param offsetCenterX The X value of the offset principal point (cx) in pixels
+     * @param targetCenterX The X value of the target's center point in pixels
+     * @param horizontalFocalLength The horizontal focal length (fx) in pixels
+     * @param offsetCenterY The Y value of the offset principal point (cy) in pixels
+     * @param targetCenterY The Y value of the target's center point in pixels
+     * @param verticalFocalLength The vertical focal length (fy) in pixels
+     * @return The yaw and pitch from the principal axis to the target center, in degrees.
+     */
+    public static DoubleCouple calculateYawPitch(
+            double offsetCenterX,
+            double targetCenterX,
+            double horizontalFocalLength,
+            double offsetCenterY,
+            double targetCenterY,
+            double verticalFocalLength) {
+        double yaw = Math.atan((targetCenterX - offsetCenterX) / horizontalFocalLength);
+        double pitch =
+                Math.atan((offsetCenterY - targetCenterY) / (verticalFocalLength / Math.cos(yaw)));
+        return new DoubleCouple(Math.toDegrees(yaw), Math.toDegrees(pitch));
     }
 
     public static double calculateSkew(boolean isLandscape, RotatedRect minAreaRect) {
diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java
index 821fed6b93..de6635b588 100644
--- a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java
+++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java
@@ -25,37 +25,50 @@
 import org.opencv.core.MatOfPoint3f;
 import org.opencv.core.Point3;
 import org.photonvision.vision.opencv.Releasable;
-
+import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
+import org.photonvision.vision.pipe.impl.SolvePNPPipe;
+
+/**
+ * A model representing the vertices of targets with known shapes. The vertices are in the EDN
+ * coordinate system. When creating a TargetModel, the vertices must be supplied in a certain order
+ * to ensure correct correspondence with corners detected in 2D for use with SolvePNP. For planar
+ * targets, we expect the target's Z-axis to point towards the camera.
+ *
+ * <p>{@link SolvePNPPipe} expects 3d object points to correspond to the {@link CornerDetectionPipe}
+ * implementation. The 2d corner detection finds the 4 extreme corners (bottom-left, bottom-right,
+ * top-right, top-left). To match our expectations, this means the model vertices would look like:
+ *
+ * <ul>
+ *   <li>(+x, +y, 0)
+ *   <li>(-x, +y, 0)
+ *   <li>(-x, -y, 0)
+ *   <li>(+x, -y, 0)
+ * </ul>
+ *
+ * <p>AprilTag models are currently only used for drawing on the output stream.
+ */
 public enum TargetModel implements Releasable {
-    k2020HighGoalOuter(
-            List.of(
-                    new Point3(Units.inchesToMeters(-19.625), 0, 0),
-                    new Point3(Units.inchesToMeters(-9.819867), Units.inchesToMeters(-17), 0),
-                    new Point3(Units.inchesToMeters(9.819867), Units.inchesToMeters(-17), 0),
-                    new Point3(Units.inchesToMeters(19.625), 0, 0)),
-            Units.inchesToMeters(12)),
-    k2020HighGoalInner(
+    k2016HighGoal(
             List.of(
-                    new Point3(Units.inchesToMeters(-19.625), 0, Units.inchesToMeters(2d * 12d + 5.25)),
-                    new Point3(
-                            Units.inchesToMeters(-9.819867),
-                            Units.inchesToMeters(-17),
-                            Units.inchesToMeters(2d * 12d + 5.25)),
-                    new Point3(
-                            Units.inchesToMeters(9.819867),
-                            Units.inchesToMeters(-17),
-                            Units.inchesToMeters(2d * 12d + 5.25)),
-                    new Point3(Units.inchesToMeters(19.625), 0, Units.inchesToMeters(2d * 12d + 5.25))),
-            Units.inchesToMeters(12)),
-
+                    new Point3(Units.inchesToMeters(10), Units.inchesToMeters(6), 0),
+                    new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(6), 0),
+                    new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(-6), 0),
+                    new Point3(Units.inchesToMeters(10), Units.inchesToMeters(-6), 0)),
+            Units.inchesToMeters(6)),
     k2019DualTarget(
             List.of(
-                    new Point3(Units.inchesToMeters(-5.936), Units.inchesToMeters(2.662), 0),
-                    new Point3(Units.inchesToMeters(-7.313), Units.inchesToMeters(-2.662), 0),
-                    new Point3(Units.inchesToMeters(7.313), Units.inchesToMeters(-2.662), 0),
-                    new Point3(Units.inchesToMeters(5.936), Units.inchesToMeters(2.662), 0)),
+                    new Point3(Units.inchesToMeters(7.313), Units.inchesToMeters(2.662), 0),
+                    new Point3(Units.inchesToMeters(-7.313), Units.inchesToMeters(2.662), 0),
+                    new Point3(Units.inchesToMeters(-5.936), Units.inchesToMeters(-2.662), 0),
+                    new Point3(Units.inchesToMeters(5.936), Units.inchesToMeters(-2.662), 0)),
             0.1),
-
+    k2020HighGoalOuter(
+            List.of(
+                    new Point3(Units.inchesToMeters(9.819867), Units.inchesToMeters(8.5), 0),
+                    new Point3(Units.inchesToMeters(-9.819867), Units.inchesToMeters(8.5), 0),
+                    new Point3(Units.inchesToMeters(-19.625), Units.inchesToMeters(-8.5), 0),
+                    new Point3(Units.inchesToMeters(19.625), Units.inchesToMeters(-8.5), 0)),
+            Units.inchesToMeters(12)),
     kCircularPowerCell7in(
             List.of(
                     new Point3(
@@ -94,28 +107,20 @@ public enum TargetModel implements Releasable {
                             -Units.inchesToMeters(9.5) / 2,
                             -Units.inchesToMeters(9.5) / 2)),
             0),
-    k2016HighGoal(
+    k200mmAprilTag( // Corners of the tag's inner black square (excluding white border)
             List.of(
-                    new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(12), 0),
-                    new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(0), 0),
-                    new Point3(Units.inchesToMeters(10), Units.inchesToMeters(0), 0),
-                    new Point3(Units.inchesToMeters(10), Units.inchesToMeters(12), 0)),
-            Units.inchesToMeters(6)),
-    k200mmAprilTag( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
-            // do not
-            List.of(
-                    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),
-                    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),
+                    new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)),
             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)),
+                    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;
diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
index 87c3d2b68f..1e29f6ee7f 100644
--- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
+++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
@@ -21,8 +21,16 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import java.util.HashMap;
 import java.util.List;
-import org.opencv.core.*;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+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.SerializationUtils;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
 import org.photonvision.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.CVShape;
@@ -70,13 +78,16 @@ public TrackedTarget(
             TargetCalculationParameters params) {
         m_targetOffsetPoint = new Point(tagDetection.getCenterX(), tagDetection.getCenterY());
         m_robotOffsetPoint = new Point();
-
-        m_pitch =
-                TargetCalculations.calculatePitch(
-                        tagDetection.getCenterY(), params.cameraCenterPoint.y, params.verticalFocalLength);
-        m_yaw =
-                TargetCalculations.calculateYaw(
-                        tagDetection.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
+        var yawPitch =
+                TargetCalculations.calculateYawPitch(
+                        params.cameraCenterPoint.x,
+                        tagDetection.getCenterX(),
+                        params.horizontalFocalLength,
+                        params.cameraCenterPoint.y,
+                        tagDetection.getCenterY(),
+                        params.verticalFocalLength);
+        m_yaw = yawPitch.getFirst();
+        m_pitch = yawPitch.getSecond();
         var bestPose = new Transform3d();
         var altPose = new Transform3d();
 
@@ -96,6 +107,22 @@ public TrackedTarget(
             m_altCameraToTarget3d = altPose;
 
             m_poseAmbiguity = tagPose.getAmbiguity();
+
+            var tvec = new Mat(3, 1, CvType.CV_64FC1);
+            tvec.put(
+                    0,
+                    0,
+                    new double[] {
+                        bestPose.getTranslation().getX(),
+                        bestPose.getTranslation().getY(),
+                        bestPose.getTranslation().getZ()
+                    });
+            setCameraRelativeTvec(tvec);
+
+            // Opencv expects a 3d vector with norm = angle and direction = axis
+            var rvec = new Mat(3, 1, CvType.CV_64FC1);
+            MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
+            setCameraRelativeRvec(rvec);
         }
 
         double[] corners = tagDetection.getCorners();
@@ -121,9 +148,11 @@ public TrackedTarget(
         tvec.put(
                 0,
                 0,
-                bestPose.getTranslation().getX(),
-                bestPose.getTranslation().getY(),
-                bestPose.getTranslation().getZ());
+                new double[] {
+                    bestPose.getTranslation().getX(),
+                    bestPose.getTranslation().getY(),
+                    bestPose.getTranslation().getZ()
+                });
         setCameraRelativeTvec(tvec);
 
         // Opencv expects a 3d vector with norm = angle and direction = axis
@@ -138,13 +167,16 @@ public TrackedTarget(
             TargetCalculationParameters params) {
         m_targetOffsetPoint = new Point(result.getCenterX(), result.getCenterY());
         m_robotOffsetPoint = new Point();
-
-        m_pitch =
-                TargetCalculations.calculatePitch(
-                        result.getCenterY(), params.cameraCenterPoint.y, params.verticalFocalLength);
-        m_yaw =
-                TargetCalculations.calculateYaw(
-                        result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
+        var yawPitch =
+                TargetCalculations.calculateYawPitch(
+                        params.cameraCenterPoint.x,
+                        result.getCenterX(),
+                        params.horizontalFocalLength,
+                        params.cameraCenterPoint.y,
+                        result.getCenterY(),
+                        params.verticalFocalLength);
+        m_yaw = yawPitch.getFirst();
+        m_pitch = yawPitch.getSecond();
 
         double[] xCorners = result.getXCorners();
         double[] yCorners = result.getYCorners();
@@ -259,7 +291,7 @@ public MatOfPoint2f getApproximateBoundingPolygon() {
     }
 
     public void calculateValues(TargetCalculationParameters params) {
-        // this MUST happen in this exact order!
+        // this MUST happen in this exact order! (TODO: document why)
         m_targetOffsetPoint =
                 TargetCalculations.calculateTargetOffsetPoint(
                         params.isLandscape, params.targetOffsetPointEdge, getMinAreaRect());
@@ -270,13 +302,18 @@ public void calculateValues(TargetCalculationParameters params) {
                         params.dualOffsetValues,
                         params.robotOffsetPointMode);
 
-        // order of this stuff doesn't matter though
-        m_pitch =
-                TargetCalculations.calculatePitch(
-                        m_targetOffsetPoint.y, m_robotOffsetPoint.y, params.verticalFocalLength);
-        m_yaw =
-                TargetCalculations.calculateYaw(
-                        m_targetOffsetPoint.x, m_robotOffsetPoint.x, params.horizontalFocalLength);
+        // order of this stuff doesnt matter though
+        var yawPitch =
+                TargetCalculations.calculateYawPitch(
+                        m_robotOffsetPoint.x,
+                        m_targetOffsetPoint.x,
+                        params.horizontalFocalLength,
+                        m_robotOffsetPoint.y,
+                        m_targetOffsetPoint.y,
+                        params.verticalFocalLength);
+        m_yaw = yawPitch.getFirst();
+        m_pitch = yawPitch.getSecond();
+
         m_area = m_mainContour.getMinAreaRect().size.area() / params.imageArea * 100;
 
         m_skew = TargetCalculations.calculateSkew(params.isLandscape, getMinAreaRect());
@@ -358,31 +395,54 @@ public HashMap<String, Object> toHashMap() {
         ret.put("skew", getSkew());
         ret.put("area", getArea());
         ret.put("ambiguity", getPoseAmbiguity());
-        if (getBestCameraToTarget3d() != null) {
-            ret.put("pose", transformToMap(getBestCameraToTarget3d()));
+
+        var bestCameraToTarget3d = getBestCameraToTarget3d();
+        if (bestCameraToTarget3d != null) {
+            ret.put("pose", SerializationUtils.transformToHashMap(bestCameraToTarget3d));
         }
         ret.put("fiducialId", getFiducialId());
         return ret;
     }
 
-    private static HashMap<String, Object> transformToMap(Transform3d transform) {
-        var ret = new HashMap<String, Object>();
-        ret.put("x", transform.getTranslation().getX());
-        ret.put("y", transform.getTranslation().getY());
-        ret.put("z", transform.getTranslation().getZ());
-        ret.put("qw", transform.getRotation().getQuaternion().getW());
-        ret.put("qx", transform.getRotation().getQuaternion().getX());
-        ret.put("qy", transform.getRotation().getQuaternion().getY());
-        ret.put("qz", transform.getRotation().getQuaternion().getZ());
-
-        ret.put("angle_z", transform.getRotation().getZ());
-        return ret;
-    }
-
     public boolean isFiducial() {
         return this.m_fiducialId >= 0;
     }
 
+    public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
+        var ret = new ArrayList<PhotonTrackedTarget>();
+        for (var t : targets) {
+            var minAreaRectCorners = new ArrayList<TargetCorner>();
+            var detectedCorners = new ArrayList<TargetCorner>();
+            {
+                var points = new Point[4];
+                t.getMinAreaRect().points(points);
+                for (int i = 0; i < 4; i++) {
+                    minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y));
+                }
+            }
+            {
+                var points = t.getTargetCorners();
+                for (int i = 0; i < points.size(); i++) {
+                    detectedCorners.add(new TargetCorner(points.get(i).x, points.get(i).y));
+                }
+            }
+
+            ret.add(
+                    new PhotonTrackedTarget(
+                            t.getYaw(),
+                            t.getPitch(),
+                            t.getArea(),
+                            t.getSkew(),
+                            t.getFiducialId(),
+                            t.getBestCameraToTarget3d(),
+                            t.getAltCameraToTarget3d(),
+                            t.getPoseAmbiguity(),
+                            minAreaRectCorners,
+                            detectedCorners));
+        }
+        return ret;
+    }
+
     public static class TargetCalculationParameters {
         // TargetOffset calculation values
         final boolean isLandscape;
diff --git a/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java b/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java
new file mode 100644
index 0000000000..a452f517f5
--- /dev/null
+++ b/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java
@@ -0,0 +1,79 @@
+/*
+ * 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.common.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.Test;
+import org.photonvision.common.util.math.MathUtils;
+
+public class CoordinateConversionTest {
+    @BeforeAll
+    public static void Init() {
+        TestUtils.loadLibraries();
+    }
+
+    @Test
+    public void testAprilTagToOpenCV() {
+        // AprilTag and OpenCV both use the EDN coordinate system. AprilTag, however, assumes the tag's
+        // z-axis points away from the camera while we expect it to point towards the camera.
+        var apriltag =
+                new Transform3d(
+                        new Translation3d(1, 2, 3),
+                        new Rotation3d(Math.toRadians(5), Math.toRadians(10), Math.toRadians(15)));
+        var opencv = MathUtils.convertApriltagtoOpenCV(apriltag);
+        final var expectedTrl = new Translation3d(1, 2, 3);
+        assertEquals(
+                expectedTrl, opencv.getTranslation(), "AprilTag to OpenCV translation conversion failed");
+        var apriltagXaxis = new Translation3d(1, 0, 0).rotateBy(apriltag.getRotation());
+        var apriltagYaxis = new Translation3d(0, 1, 0).rotateBy(apriltag.getRotation());
+        var apriltagZaxis = new Translation3d(0, 0, 1).rotateBy(apriltag.getRotation());
+        var opencvXaxis = new Translation3d(1, 0, 0).rotateBy(opencv.getRotation());
+        var opencvYaxis = new Translation3d(0, 1, 0).rotateBy(opencv.getRotation());
+        var opencvZaxis = new Translation3d(0, 0, 1).rotateBy(opencv.getRotation());
+        assertEquals(
+                apriltagXaxis.unaryMinus(),
+                opencvXaxis,
+                "AprilTag to OpenCV rotation conversion failed(X-axis)");
+        assertEquals(
+                apriltagYaxis, opencvYaxis, "AprilTag to OpenCV rotation conversion failed(Y-axis)");
+        assertEquals(
+                apriltagZaxis.unaryMinus(),
+                opencvZaxis,
+                "AprilTag to OpenCV rotation conversion failed(Z-axis)");
+    }
+
+    @Test
+    public void testOpenCVToPhoton() {
+        // OpenCV uses the EDN coordinate system while wpilib is in NWU.
+        var opencv =
+                new Transform3d(
+                        new Translation3d(1, 2, 3), new Rotation3d(VecBuilder.fill(1, 2, 3), Math.PI / 8));
+        var wpilib = MathUtils.convertOpenCVtoPhotonTransform(opencv);
+        final var expectedTrl = new Translation3d(3, -1, -2);
+        assertEquals(
+                expectedTrl, wpilib.getTranslation(), "OpenCV to WPILib translation conversion failed");
+        var expectedRot = new Rotation3d(VecBuilder.fill(3, -1, -2), Math.PI / 8);
+        assertEquals(expectedRot, wpilib.getRotation(), "OpenCV to WPILib rotation conversion failed");
+    }
+}
diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
index 1ece69fc55..fe1b7b9d9c 100644
--- a/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
+++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
@@ -75,20 +75,19 @@ public void testApriltagFacingCamera() {
         // 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.1, pose.getTranslation().getY(), 0.2);
         Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2);
 
-        var objX = new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY();
-        var objY = new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ();
-        var objZ = new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX();
+        // We expect the object axes to be in NWU, with the x-axis coming out of the tag
+        // This visible tag is facing the camera almost parallel, so in world space:
 
-        // We expect the object X to be forward, or -X in world space
+        // The object's X axis should be (-1, 0, 0)
         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
+        // The object's Y axis should be (0, -1, 0)
         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
+        // The object's Z axis should be (0, 0, 1)
         Assertions.assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.1);
     }
 
diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java
index 02852308b9..03bcc2535d 100644
--- a/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java
+++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java
@@ -19,11 +19,12 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 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;
@@ -115,28 +116,31 @@ public void test2019() {
                         pipeline.getSettings().hueInverted);
         frameProvider.requestHsvSettings(hsvParams);
 
-        CVPipelineResult pipelineResult;
-
-        pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+        CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
         printTestResults(pipelineResult);
 
-        // these numbers are not *accurate*, but they are known and expected
-        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 to be forward, or -X in world space
-        Assertions.assertEquals(
-                -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()).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()).getZ(), 0.05);
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        outputPipe.process(
+                pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
 
         TestUtils.showImage(
-                pipelineResult.inputAndOutputFrame.colorImage.getMat(), "Pipeline output", 999999);
+                pipelineResult.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
+
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
+        // these numbers are not *accurate*, but they are known and expected
+        var expectedTrl = new Translation3d(1.1, -0.05, -0.05);
+        assertTrue(
+                expectedTrl.getDistance(pose.getTranslation()) < 0.05,
+                "SolvePNP translation estimation failed");
+        // We expect the object axes to be in NWU, with the x-axis coming out of the tag
+        // This target is facing the camera almost parallel, so in world space:
+        // The object's X axis should be (-1, 0, 0)
+        assertEquals(-1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.05);
+        // The object's Y axis should be (0, -1, 0)
+        assertEquals(-1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.05);
+        // The object's Z axis should be (0, 0, 1)
+        assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.05);
     }
 
     @Test
@@ -175,15 +179,27 @@ public void test2020() {
         outputPipe.process(
                 pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
 
-        // these numbers are not *accurate*, but they are known and expected
-        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);
-        // Z rotation should be mostly facing us
-        Assertions.assertEquals(Units.degreesToRadians(-140), pose.getRotation().getZ(), 1);
-
         TestUtils.showImage(
-                pipelineResult.inputAndOutputFrame.colorImage.getMat(), "Pipeline output", 999999);
+                pipelineResult.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
+
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
+        // these numbers are not *accurate*, but they are known and expected
+        var expectedTrl =
+                new Translation3d(
+                        Units.inchesToMeters(236), Units.inchesToMeters(36), Units.inchesToMeters(-53));
+        assertTrue(
+                expectedTrl.getDistance(pose.getTranslation()) < 0.05,
+                "SolvePNP translation estimation failed");
+        // We expect the object axes to be in NWU, with the x-axis coming out of the tag
+        // Rotation around Z axis (yaw) should be mostly facing us
+        var xAxis = new Translation3d(1, 0, 0);
+        var yAxis = new Translation3d(0, 1, 0);
+        var zAxis = new Translation3d(0, 0, 1);
+        var expectedRot =
+                new Rotation3d(Math.toRadians(-20), Math.toRadians(-20), Math.toRadians(-120));
+        assertTrue(xAxis.rotateBy(expectedRot).getDistance(xAxis.rotateBy(pose.getRotation())) < 0.1);
+        assertTrue(yAxis.rotateBy(expectedRot).getDistance(yAxis.rotateBy(pose.getRotation())) < 0.1);
+        assertTrue(zAxis.rotateBy(expectedRot).getDistance(zAxis.rotateBy(pose.getRotation())) < 0.1);
     }
 
     private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) {
diff --git a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java
index 0dd36d90e9..7b23d3a600 100644
--- a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java
+++ b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java
@@ -17,14 +17,19 @@
 package org.photonvision.vision.target;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.util.stream.Stream;
 import org.junit.jupiter.api.Assertions;
-import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
-import org.opencv.core.MatOfPoint2f;
-import org.opencv.core.Point;
-import org.opencv.core.RotatedRect;
-import org.opencv.core.Size;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.*;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.common.util.numbers.DoubleCouple;
@@ -33,9 +38,9 @@
 
 public class TargetCalculationsTest {
 
-    private static final Size imageSize = new Size(800, 600);
-    private static final Point imageCenterPoint =
-            new Point(imageSize.width / 2, imageSize.height / 2);
+    private static Size imageSize = new Size(800, 600);
+    private static Point imageCenterPoint =
+            new Point(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5);
     private static final double diagFOV = Math.toRadians(70.0);
 
     private static final FrameStaticProperties props =
@@ -52,43 +57,104 @@ public class TargetCalculationsTest {
                     props.verticalFocalLength,
                     imageSize.width * imageSize.height);
 
-    @BeforeEach
-    public void Init() {
+    @BeforeAll
+    public static void setup() {
         TestUtils.loadLibraries();
     }
 
     @Test
-    public void yawTest() {
-        var targetPixelOffsetX = 100;
-        var targetCenterPoint = new Point(imageCenterPoint.x + targetPixelOffsetX, imageCenterPoint.y);
-
-        var trueYaw =
-                Math.atan((imageCenterPoint.x - targetCenterPoint.x) / params.horizontalFocalLength);
-
-        var yaw =
-                TargetCalculations.calculateYaw(
-                        imageCenterPoint.x, targetCenterPoint.x, params.horizontalFocalLength);
-
-        assertEquals(Math.toDegrees(trueYaw), yaw, 0.025, "Yaw not as expected");
+    public void testYawPitchBehavior() {
+        double targetPixelOffsetX = 100;
+        double targetPixelOffsetY = 100;
+        var targetCenterPoint =
+                new Point(imageCenterPoint.x + targetPixelOffsetX, imageCenterPoint.y + targetPixelOffsetY);
+
+        var targetYawPitch =
+                TargetCalculations.calculateYawPitch(
+                        imageCenterPoint.x,
+                        targetCenterPoint.x,
+                        params.horizontalFocalLength,
+                        imageCenterPoint.y,
+                        targetCenterPoint.y,
+                        params.verticalFocalLength);
+
+        assertTrue(targetYawPitch.getFirst() > 0, "Yaw is not positive right");
+        assertTrue(targetYawPitch.getSecond() < 0, "Pitch is not positive up");
+
+        var fovs =
+                FrameStaticProperties.calculateHorizontalVerticalFoV(
+                        diagFOV, (int) imageSize.width, (int) imageSize.height);
+        var maxYaw =
+                TargetCalculations.calculateYawPitch(
+                        imageCenterPoint.x,
+                        2 * imageCenterPoint.x,
+                        params.horizontalFocalLength,
+                        imageCenterPoint.y,
+                        imageCenterPoint.y,
+                        params.verticalFocalLength);
+        assertEquals(fovs.getFirst() / 2.0, maxYaw.getFirst(), 0.025, "Horizontal FOV check failed");
+        var maxPitch =
+                TargetCalculations.calculateYawPitch(
+                        imageCenterPoint.x,
+                        imageCenterPoint.x,
+                        params.horizontalFocalLength,
+                        imageCenterPoint.y,
+                        0,
+                        params.verticalFocalLength);
+        assertEquals(fovs.getSecond() / 2.0, maxPitch.getSecond(), 0.025, "Vertical FOV check failed");
     }
 
-    @Test
-    public void pitchTest() {
-        var targetPixelOffsetY = 100;
-        var targetCenterPoint = new Point(imageCenterPoint.x, imageCenterPoint.y + targetPixelOffsetY);
-
-        var truePitch =
-                Math.atan((imageCenterPoint.y - targetCenterPoint.y) / params.verticalFocalLength);
-
-        var pitch =
-                TargetCalculations.calculatePitch(
-                        imageCenterPoint.y, targetCenterPoint.y, params.verticalFocalLength);
+    private static Stream<Arguments> testYawPitchCalcArgs() {
+        return Stream.of(
+                // (yaw, pitch) in degrees
+                Arguments.of(0, 0),
+                Arguments.of(10, 0),
+                Arguments.of(0, 10),
+                Arguments.of(10, 10),
+                Arguments.of(-10, -10),
+                Arguments.of(30, 45),
+                Arguments.of(-45, -20));
+    }
 
-        assertEquals(Math.toDegrees(truePitch) * -1, pitch, 0.025, "Pitch not as expected");
+    private static double[] testCameraMatrix = {240, 0, 320, 0, 240, 320, 0, 0, 1};
+
+    @ParameterizedTest
+    @MethodSource("testYawPitchCalcArgs")
+    public void testYawPitchCalc(double yawDeg, double pitchDeg) {
+        Mat testCameraMat = new Mat(3, 3, CvType.CV_64F);
+        testCameraMat.put(0, 0, testCameraMatrix);
+        // Since we create this translation using the given yaw/pitch, we should see the same angles
+        // calculated
+        var targetTrl =
+                new Translation3d(1, new Rotation3d(0, Math.toRadians(pitchDeg), Math.toRadians(yawDeg)));
+        // NWU to EDN
+        var objectPoints =
+                new MatOfPoint3f(new Point3(-targetTrl.getY(), -targetTrl.getZ(), targetTrl.getX()));
+        var imagePoints = new MatOfPoint2f();
+        // Project translation into camera image
+        Calib3d.projectPoints(
+                objectPoints,
+                new MatOfDouble(0, 0, 0),
+                new MatOfDouble(0, 0, 0),
+                testCameraMat,
+                new MatOfDouble(0, 0, 0, 0, 0),
+                imagePoints);
+        var point = imagePoints.toArray()[0];
+        // Test if the target yaw/pitch calculation matches what the target was created with
+        var yawPitch =
+                TargetCalculations.calculateYawPitch(
+                        point.x,
+                        testCameraMatrix[2],
+                        testCameraMatrix[0],
+                        point.y,
+                        testCameraMatrix[5],
+                        testCameraMatrix[4]);
+        assertEquals(yawDeg, yawPitch.getFirst(), 1e-3, "Yaw calculation incorrect");
+        assertEquals(pitchDeg, yawPitch.getSecond(), 1e-3, "Pitch calculation incorrect");
     }
 
     @Test
-    public void targetOffsetTest() {
+    public void testTargetOffset() {
         Point center = new Point(0, 0);
         Size rectSize = new Size(10, 5);
         double angle = 30;
@@ -105,11 +171,6 @@ public void targetOffsetTest() {
         assertEquals(2.17, result.y, 0.1, "Target offset Y not as expected");
     }
 
-    public static void main(String[] args) {
-        TestUtils.loadLibraries();
-        new TargetCalculationsTest().targetOffsetTest();
-    }
-
     @Test
     public void testSkewCalculation() {
         // Setup
@@ -191,14 +252,14 @@ public void testSkewCalculation() {
     public void testCameraFOVCalculation() {
         final DoubleCouple glowormHorizVert =
                 FrameStaticProperties.calculateHorizontalVerticalFoV(74.8, 640, 480);
-        var gwHorizDeg = Math.toDegrees(glowormHorizVert.getFirst());
-        var gwVertDeg = Math.toDegrees(glowormHorizVert.getSecond());
+        var gwHorizDeg = glowormHorizVert.getFirst();
+        var gwVertDeg = glowormHorizVert.getSecond();
         assertEquals(62.7, gwHorizDeg, .3);
         assertEquals(49, gwVertDeg, .3);
     }
 
     @Test
-    public void robotOffsetDualTest() {
+    public void testDualOffsetCrosshair() {
         final DualOffsetValues dualOffsetValues =
                 new DualOffsetValues(
                         new Point(400, 150), 10,
diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle
index d7952394df..baae8fcc7d 100644
--- a/photon-lib/build.gradle
+++ b/photon-lib/build.gradle
@@ -120,23 +120,22 @@ task generateVendorJson() {
     outputs.file photonlibFileOutput
     inputs.file photonlibFileInput
 
-    doLast {
-        println "Writing version ${pubVersion} to $photonlibFileOutput"
+        println "Writing vendor JSON ${pubVersion} to $photonlibFileOutput"
 
         if (photonlibFileOutput.exists()) {
             photonlibFileOutput.delete()
         }
+        photonlibFileOutput.parentFile.mkdirs()
+
         def read = photonlibFileInput.text
             .replace('${photon_version}', pubVersion)
             .replace('${frc_year}', frcYear)
-        photonlibFileOutput.write(read)
-    }
+        photonlibFileOutput.text = read
 
     outputs.upToDateWhen { false }
 }
 
-build.dependsOn generateVendorJson
-
+build.mustRunAfter generateVendorJson
 
 task writeCurrentVersion {
     def versionFileIn = file("${rootDir}/shared/PhotonVersion.java.in")
@@ -147,7 +146,7 @@ task writeCurrentVersion {
             versionString)
 }
 
-build.dependsOn writeCurrentVersion
+build.mustRunAfter writeCurrentVersion
 
 tasks.withType(Javadoc) {
     options.encoding = 'UTF-8'
diff --git a/photon-lib/publish.gradle b/photon-lib/publish.gradle
index 9967b24ee2..ca67c43b0e 100644
--- a/photon-lib/publish.gradle
+++ b/photon-lib/publish.gradle
@@ -198,8 +198,7 @@ model {
     }
 }
 
-// So I don't actually know the _right_ way to tell gradle that the vendordep json publish requires generation first, so we're doing this
-getTasksByName("publishVendorjsonPublicationToMavenLocal", false).each {
+tasks.withType(PublishToMavenRepository) {
     it.mustRunAfter generateVendorJson
 }
 
diff --git a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
index 7d9f9b6dcd..a355ab00f3 100644
--- a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
+++ b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
@@ -26,6 +26,7 @@
 
 import edu.wpi.first.math.geometry.Pose3d;
 import java.util.List;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 /** An estimated pose based on pipeline result */
@@ -39,6 +40,9 @@ public class EstimatedRobotPose {
     /** A list of the targets used to compute this pose */
     public final List<PhotonTrackedTarget> targetsUsed;
 
+    /** The strategy actually used to produce this pose */
+    public final PoseStrategy strategy;
+
     /**
      * Constructs an EstimatedRobotPose
      *
@@ -46,9 +50,13 @@ public class EstimatedRobotPose {
      * @param timestampSeconds timestamp of the estimate
      */
     public EstimatedRobotPose(
-            Pose3d estimatedPose, double timestampSeconds, List<PhotonTrackedTarget> targetsUsed) {
+            Pose3d estimatedPose,
+            double timestampSeconds,
+            List<PhotonTrackedTarget> targetsUsed,
+            PoseStrategy strategy) {
         this.estimatedPose = estimatedPose;
         this.timestampSeconds = timestampSeconds;
         this.targetsUsed = targetsUsed;
+        this.strategy = strategy;
     }
 }
diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
index b9978a8021..9500629ca3 100644
--- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
@@ -24,7 +24,11 @@
 
 package org.photonvision;
 
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.numbers.*;
@@ -41,6 +45,7 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.PubSubOption;
 import edu.wpi.first.networktables.RawSubscriber;
+import edu.wpi.first.networktables.StringPublisher;
 import edu.wpi.first.networktables.StringSubscriber;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
@@ -72,6 +77,7 @@ public class PhotonCamera implements AutoCloseable {
     IntegerSubscriber heartbeatEntry;
     private DoubleArraySubscriber cameraIntrinsicsSubscriber;
     private DoubleArraySubscriber cameraDistortionSubscriber;
+    private StringPublisher atflPublisher;
 
     @Override
     public void close() {
@@ -95,6 +101,7 @@ public void close() {
         pipelineIndexRequest.close();
         cameraIntrinsicsSubscriber.close();
         cameraDistortionSubscriber.close();
+        atflPublisher.close();
     }
 
     private final String path;
@@ -114,6 +121,7 @@ public static void setVersionCheckEnabled(boolean enabled) {
 
     Packet packet = new Packet(1);
 
+    // Existing is enough to make this multisubscriber do its thing
     private final MultiSubscriber m_topicNameSubscriber;
 
     /**
@@ -150,6 +158,10 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1);
         versionEntry = photonvision_root_table.getStringTopic("version").subscribe("");
 
+        atflPublisher = photonvision_root_table.getStringTopic("apriltag_field_layout").publish();
+        // Save the layout locally on Rio; on reboot, should be pushed out to NT clients
+        atflPublisher.getTopic().setPersistent(true);
+
         m_topicNameSubscriber =
                 new MultiSubscriber(
                         instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
@@ -319,6 +331,28 @@ public boolean isConnected() {
         return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
     }
 
+    /**
+     * Set the Apriltag Field Layout used by all PhotonVision coprocessors that are (or might later)
+     * connect to this robot. The topic is marked as persistant, so even if you only call this once
+     * ever, it will be saved on the RoboRIO and pushed out to all NT clients when code reboots.
+     * PhotonVision will also store a copy of this layout locally on the coprocessor, but subscribes
+     * to this topic and the local copy will be updated when this function is called.
+     *
+     * @param layout The layout to use for *all* PhotonVision cameras
+     * @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients
+     *     have updated their internal layouts.
+     */
+    public boolean setApriltagFieldLayout(AprilTagFieldLayout layout) {
+        try {
+            var layout_json = new ObjectMapper().writeValueAsString(layout);
+            atflPublisher.set(layout_json);
+            return true;
+        } catch (JsonProcessingException e) {
+            MathSharedStore.reportError("Error setting ATFL in " + this.getName(), e.getStackTrace());
+        }
+        return false;
+    }
+
     public Optional<Matrix<N3, N3>> getCameraMatrix() {
         var cameraMatrix = cameraIntrinsicsSubscriber.get();
         if (cameraMatrix != null && cameraMatrix.length == 9) {
diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
index 0eecc02880..ed29a9c265 100644
--- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -68,8 +68,17 @@ public enum PoseStrategy {
         /** Return the average of the best target poses using ambiguity as weight. */
         AVERAGE_BEST_TARGETS,
 
-        /** Use all visible tags to compute a single pose estimate.. */
-        MULTI_TAG_PNP
+        /**
+         * Use all visible tags to compute a single pose estimate on coprocessor. This option needs to
+         * be enabled on the PhotonVision web UI as well.
+         */
+        MULTI_TAG_PNP_ON_COPROCESSOR,
+
+        /**
+         * Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can
+         * take a lot of time.
+         */
+        MULTI_TAG_PNP_ON_RIO
     }
 
     private AprilTagFieldLayout fieldTags;
@@ -173,7 +182,8 @@ public void setPrimaryStrategy(PoseStrategy strategy) {
      */
     public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
         checkUpdate(this.multiTagFallbackStrategy, strategy);
-        if (strategy == PoseStrategy.MULTI_TAG_PNP) {
+        if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
+                || strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) {
             DriverStation.reportWarning(
                     "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false);
             strategy = PoseStrategy.LOWEST_AMBIGUITY;
@@ -357,8 +367,11 @@ private Optional<EstimatedRobotPose> update(
             case AVERAGE_BEST_TARGETS:
                 estimatedPose = averageBestTargetsStrategy(cameraResult);
                 break;
-            case MULTI_TAG_PNP:
-                estimatedPose = multiTagPNPStrategy(cameraResult, cameraMatrix, distCoeffs);
+            case MULTI_TAG_PNP_ON_RIO:
+                estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
+                break;
+            case MULTI_TAG_PNP_ON_COPROCESSOR:
+                estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs);
                 break;
             default:
                 DriverStation.reportError(
@@ -373,7 +386,28 @@ private Optional<EstimatedRobotPose> update(
         return estimatedPose;
     }
 
-    private Optional<EstimatedRobotPose> multiTagPNPStrategy(
+    private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
+            PhotonPipelineResult result,
+            Optional<Matrix<N3, N3>> cameraMatrixOpt,
+            Optional<Matrix<N5, N1>> distCoeffsOpt) {
+        if (result.getMultiTagResult().estimatedPose.isPresent) {
+            var best_tf = result.getMultiTagResult().estimatedPose.best;
+            var best =
+                    new Pose3d()
+                            .plus(best_tf) // field-to-camera
+                            .plus(robotToCamera.inverse()); // field-to-robot
+            return Optional.of(
+                    new EstimatedRobotPose(
+                            best,
+                            result.getTimestampSeconds(),
+                            result.getTargets(),
+                            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
+        } else {
+            return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
+        }
+    }
+
+    private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
             PhotonPipelineResult result,
             Optional<Matrix<N3, N3>> cameraMatrixOpt,
             Optional<Matrix<N5, N1>> distCoeffsOpt) {
@@ -395,7 +429,11 @@ private Optional<EstimatedRobotPose> multiTagPNPStrategy(
                         .plus(robotToCamera.inverse()); // field-to-robot
 
         return Optional.of(
-                new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets()));
+                new EstimatedRobotPose(
+                        best,
+                        result.getTimestampSeconds(),
+                        result.getTargets(),
+                        PoseStrategy.MULTI_TAG_PNP_ON_RIO));
     }
 
     /**
@@ -440,7 +478,8 @@ private Optional<EstimatedRobotPose> lowestAmbiguityStrategy(PhotonPipelineResul
                                 .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
                                 .transformBy(robotToCamera.inverse()),
                         result.getTimestampSeconds(),
-                        result.getTargets()));
+                        result.getTargets(),
+                        PoseStrategy.LOWEST_AMBIGUITY));
     }
 
     /**
@@ -494,7 +533,8 @@ private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelin
                                         .transformBy(target.getAlternateCameraToTarget().inverse())
                                         .transformBy(robotToCamera.inverse()),
                                 result.getTimestampSeconds(),
-                                result.getTargets());
+                                result.getTargets(),
+                                PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
             }
 
             if (bestTransformDelta < smallestHeightDifference) {
@@ -506,7 +546,8 @@ private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelin
                                         .transformBy(target.getBestCameraToTarget().inverse())
                                         .transformBy(robotToCamera.inverse()),
                                 result.getTimestampSeconds(),
-                                result.getTargets());
+                                result.getTargets(),
+                                PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
             }
         }
 
@@ -568,13 +609,19 @@ private Optional<EstimatedRobotPose> closestToReferencePoseStrategy(
                 smallestPoseDelta = altDifference;
                 lowestDeltaPose =
                         new EstimatedRobotPose(
-                                altTransformPosition, result.getTimestampSeconds(), result.getTargets());
+                                altTransformPosition,
+                                result.getTimestampSeconds(),
+                                result.getTargets(),
+                                PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
             }
             if (bestDifference < smallestPoseDelta) {
                 smallestPoseDelta = bestDifference;
                 lowestDeltaPose =
                         new EstimatedRobotPose(
-                                bestTransformPosition, result.getTimestampSeconds(), result.getTargets());
+                                bestTransformPosition,
+                                result.getTimestampSeconds(),
+                                result.getTargets(),
+                                PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
             }
         }
         return Optional.ofNullable(lowestDeltaPose);
@@ -617,7 +664,8 @@ private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineRe
                                         .transformBy(target.getBestCameraToTarget().inverse())
                                         .transformBy(robotToCamera.inverse()),
                                 result.getTimestampSeconds(),
-                                result.getTargets()));
+                                result.getTargets(),
+                                PoseStrategy.AVERAGE_BEST_TARGETS));
             }
 
             totalAmbiguity += 1.0 / target.getPoseAmbiguity();
@@ -649,7 +697,10 @@ private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineRe
 
         return Optional.of(
                 new EstimatedRobotPose(
-                        new Pose3d(transform, rotation), result.getTimestampSeconds(), result.getTargets()));
+                        new Pose3d(transform, rotation),
+                        result.getTimestampSeconds(),
+                        result.getTargets(),
+                        PoseStrategy.AVERAGE_BEST_TARGETS));
     }
 
     /**
diff --git a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java
deleted file mode 100644
index 4dd6dcdaaa..0000000000
--- a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java
+++ /dev/null
@@ -1,419 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-package org.photonvision;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.math.Pair;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.wpilibj.DriverStation;
-import java.util.ArrayList;
-import java.util.HashSet;
-import java.util.List;
-import java.util.Optional;
-import java.util.Set;
-import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
-
-/**
- * @deprecated Use {@link PhotonPoseEstimator}
- */
-@Deprecated
-public class RobotPoseEstimator {
-    /**
-     *
-     *
-     * <ul>
-     *   <li><strong>LOWEST_AMBIGUITY</strong>: Choose the Pose with the lowest ambiguity
-     *   <li><strong>CLOSEST_TO_CAMERA_HEIGHT</strong>: Choose the Pose which is closest to the camera
-     *       height
-     *   <li><strong>CLOSEST_TO_REFERENCE_POSE</strong>: Choose the Pose which is closest to the pose
-     *       from setReferencePose()
-     *   <li><strong>CLOSEST_TO_LAST_POSE</strong>: Choose the Pose which is closest to the last pose
-     *       calculated
-     * </ul>
-     */
-    public enum PoseStrategy {
-        LOWEST_AMBIGUITY,
-        CLOSEST_TO_CAMERA_HEIGHT,
-        CLOSEST_TO_REFERENCE_POSE,
-        CLOSEST_TO_LAST_POSE,
-        AVERAGE_BEST_TARGETS
-    }
-
-    private AprilTagFieldLayout aprilTags;
-    private PoseStrategy strategy;
-    private List<Pair<PhotonCamera, Transform3d>> cameras;
-    private Pose3d lastPose;
-
-    private Pose3d referencePose;
-    private Set<Integer> reportedErrors;
-
-    /**
-     * Create a new RobotPoseEstimator.
-     *
-     * @param aprilTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3ds with
-     *     respect to the FIRST field.
-     * @param strategy The strategy it should use to determine the best pose.
-     * @param cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from
-     *     the center of the robot to the camera mount positions (ie, robot ➔ camera).
-     */
-    public RobotPoseEstimator(
-            AprilTagFieldLayout aprilTags,
-            PoseStrategy strategy,
-            List<Pair<PhotonCamera, Transform3d>> cameras) {
-        this.aprilTags = aprilTags;
-        this.strategy = strategy;
-        this.cameras = cameras;
-        lastPose = new Pose3d();
-        reportedErrors = new HashSet<>();
-    }
-
-    /**
-     * Update the estimated pose using the selected strategy.
-     *
-     * @return The updated estimated pose and the latency in milliseconds. Estimated pose may be null
-     *     if no targets were seen
-     */
-    public Optional<Pair<Pose3d, Double>> update() {
-        if (cameras.isEmpty()) {
-            DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false);
-            return Optional.empty();
-        }
-
-        Pair<Pose3d, Double> pair = getResultFromActiveStrategy();
-
-        if (pair != null) {
-            lastPose = pair.getFirst();
-        }
-
-        return Optional.ofNullable(pair);
-    }
-
-    private Pair<Pose3d, Double> getResultFromActiveStrategy() {
-        switch (strategy) {
-            case LOWEST_AMBIGUITY:
-                return lowestAmbiguityStrategy();
-            case CLOSEST_TO_CAMERA_HEIGHT:
-                return closestToCameraHeightStrategy();
-            case CLOSEST_TO_REFERENCE_POSE:
-                return closestToReferencePoseStrategy();
-            case CLOSEST_TO_LAST_POSE:
-                return closestToLastPoseStrategy();
-            case AVERAGE_BEST_TARGETS:
-                return averageBestTargetsStrategy();
-            default:
-                DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false);
-                return null;
-        }
-    }
-
-    private Pair<Pose3d, Double> lowestAmbiguityStrategy() {
-        int lowestAI = -1;
-        int lowestAJ = -1;
-        double lowestAmbiguityScore = 10;
-        ArrayList<PhotonPipelineResult> results = new ArrayList<PhotonPipelineResult>(cameras.size());
-
-        // Sample result from each camera
-        for (int i = 0; i < cameras.size(); i++) {
-            Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            results.add(p.getFirst().getLatestResult());
-        }
-
-        // Loop over each ambiguity of all the cameras
-        for (int i = 0; i < cameras.size(); i++) {
-            List<PhotonTrackedTarget> targets = results.get(i).targets;
-            for (int j = 0; j < targets.size(); j++) {
-                if (targets.get(j).getPoseAmbiguity() < lowestAmbiguityScore) {
-                    lowestAI = i;
-                    lowestAJ = j;
-                    lowestAmbiguityScore = targets.get(j).getPoseAmbiguity();
-                }
-            }
-        }
-
-        // No targets, return null
-        if (lowestAI == -1 || lowestAJ == -1) {
-            return null;
-        }
-
-        // Pick the lowest and do the heavy calculations
-        PhotonTrackedTarget bestTarget = results.get(lowestAI).targets.get(lowestAJ);
-
-        Optional<Pose3d> fiducialPose = aprilTags.getTagPose(bestTarget.getFiducialId());
-        if (fiducialPose.isEmpty()) {
-            reportFiducialPoseError(bestTarget.getFiducialId());
-            return null;
-        }
-
-        return Pair.of(
-                fiducialPose
-                        .get()
-                        .transformBy(bestTarget.getBestCameraToTarget().inverse())
-                        .transformBy(cameras.get(lowestAI).getSecond().inverse()),
-                results.get(lowestAI).getLatencyMillis());
-    }
-
-    private Pair<Pose3d, Double> closestToCameraHeightStrategy() {
-        double smallestHeightDifference = Double.MAX_VALUE;
-        double latency = 0;
-        Pose3d pose = null;
-
-        for (int i = 0; i < cameras.size(); i++) {
-            Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            var result = p.getFirst().getLatestResult();
-            List<PhotonTrackedTarget> targets = result.targets;
-            for (int j = 0; j < targets.size(); j++) {
-                PhotonTrackedTarget target = targets.get(j);
-                Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
-                if (fiducialPose.isEmpty()) {
-                    reportFiducialPoseError(target.getFiducialId());
-                    continue;
-                }
-                Pose3d targetPose = fiducialPose.get();
-                double alternativeDifference =
-                        Math.abs(
-                                p.getSecond().getZ()
-                                        - targetPose.transformBy(target.getAlternateCameraToTarget().inverse()).getZ());
-                double bestDifference =
-                        Math.abs(
-                                p.getSecond().getZ()
-                                        - targetPose.transformBy(target.getBestCameraToTarget().inverse()).getZ());
-                if (alternativeDifference < smallestHeightDifference) {
-                    smallestHeightDifference = alternativeDifference;
-                    pose =
-                            targetPose
-                                    .transformBy(target.getAlternateCameraToTarget().inverse())
-                                    .transformBy(p.getSecond().inverse());
-                    latency = result.getLatencyMillis();
-                }
-                if (bestDifference < smallestHeightDifference) {
-                    smallestHeightDifference = bestDifference;
-                    pose =
-                            targetPose
-                                    .transformBy(target.getBestCameraToTarget().inverse())
-                                    .transformBy(p.getSecond().inverse());
-                    latency = result.getLatencyMillis();
-                }
-            }
-        }
-
-        return Pair.of(pose, latency);
-    }
-
-    private Pair<Pose3d, Double> closestToReferencePoseStrategy() {
-        if (referencePose == null) {
-            DriverStation.reportError(
-                    "[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!",
-                    false);
-            return null;
-        }
-        double smallestDifference = 10e9;
-        double latency = 0;
-        Pose3d pose = null;
-        for (int i = 0; i < cameras.size(); i++) {
-            Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            var result = p.getFirst().getLatestResult();
-            List<PhotonTrackedTarget> targets = result.targets;
-            for (int j = 0; j < targets.size(); j++) {
-                PhotonTrackedTarget target = targets.get(j);
-                Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
-                if (fiducialPose.isEmpty()) {
-                    reportFiducialPoseError(target.getFiducialId());
-                    continue;
-                }
-                Pose3d targetPose = fiducialPose.get();
-                Pose3d botBestPose =
-                        targetPose
-                                .transformBy(target.getAlternateCameraToTarget().inverse())
-                                .transformBy(p.getSecond().inverse());
-                Pose3d botAltPose =
-                        targetPose
-                                .transformBy(target.getBestCameraToTarget().inverse())
-                                .transformBy(p.getSecond().inverse());
-                double alternativeDifference = Math.abs(calculateDifference(referencePose, botAltPose));
-                double bestDifference = Math.abs(calculateDifference(referencePose, botBestPose));
-                if (alternativeDifference < smallestDifference) {
-                    smallestDifference = alternativeDifference;
-                    pose = botAltPose;
-                    latency = result.getLatencyMillis();
-                }
-                if (bestDifference < smallestDifference) {
-                    smallestDifference = bestDifference;
-                    pose = botBestPose;
-                    latency = result.getLatencyMillis();
-                }
-            }
-        }
-        return Pair.of(pose, latency);
-    }
-
-    private Pair<Pose3d, Double> closestToLastPoseStrategy() {
-        setReferencePose(lastPose);
-        return closestToReferencePoseStrategy();
-    }
-
-    /** Return the average of the best target poses using ambiguity as weight */
-    private Pair<Pose3d, Double> averageBestTargetsStrategy() {
-        //                  Pair of Double, Double = Ambiguity, Mili
-        List<Pair<Pose3d, Pair<Double, Double>>> tempPoses = new ArrayList<>();
-        double totalAmbiguity = 0;
-        for (int i = 0; i < cameras.size(); i++) {
-            Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            var result = p.getFirst().getLatestResult();
-            List<PhotonTrackedTarget> targets = result.targets;
-            for (int j = 0; j < targets.size(); j++) {
-                PhotonTrackedTarget target = targets.get(j);
-                Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
-                if (fiducialPose.isEmpty()) {
-                    reportFiducialPoseError(target.getFiducialId());
-                    continue;
-                }
-                Pose3d targetPose = fiducialPose.get();
-                try {
-                    totalAmbiguity += 1. / target.getPoseAmbiguity();
-                } catch (ArithmeticException e) {
-                    // A total ambiguity of zero exists, using that pose instead!",
-                    return Pair.of(
-                            targetPose
-                                    .transformBy(target.getBestCameraToTarget().inverse())
-                                    .transformBy(p.getSecond().inverse()),
-                            result.getLatencyMillis());
-                }
-                tempPoses.add(
-                        Pair.of(
-                                targetPose
-                                        .transformBy(target.getBestCameraToTarget().inverse())
-                                        .transformBy(p.getSecond().inverse()),
-                                Pair.of(target.getPoseAmbiguity(), result.getLatencyMillis())));
-            }
-        }
-
-        Translation3d transform = new Translation3d();
-        Rotation3d rotation = new Rotation3d();
-        double latency = 0;
-
-        if (tempPoses.isEmpty()) {
-            return null;
-        }
-
-        if (totalAmbiguity == 0) {
-            Pose3d p = tempPoses.get(0).getFirst();
-            double l = tempPoses.get(0).getSecond().getSecond();
-            return Pair.of(p, l);
-        }
-
-        for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
-            double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
-            transform = transform.plus(pair.getFirst().getTranslation().times(weight));
-            rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
-            latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
-        }
-
-        return Pair.of(new Pose3d(transform, rotation), latency);
-    }
-
-    /**
-     * Difference is defined as the vector magnitude between the two poses
-     *
-     * @return The absolute "difference" (>=0) between two Pose3ds.
-     */
-    private double calculateDifference(Pose3d x, Pose3d y) {
-        return x.getTranslation().getDistance(y.getTranslation());
-    }
-
-    /**
-     * @param aprilTags the aprilTags to set
-     */
-    public void setAprilTags(AprilTagFieldLayout aprilTags) {
-        this.aprilTags = aprilTags;
-    }
-
-    /**
-     * @return the aprilTags
-     */
-    public AprilTagFieldLayout getAprilTags() {
-        return aprilTags;
-    }
-
-    /**
-     * @return the strategy
-     */
-    public PoseStrategy getStrategy() {
-        return strategy;
-    }
-
-    /**
-     * @param strategy the strategy to set
-     */
-    public void setStrategy(PoseStrategy strategy) {
-        this.strategy = strategy;
-    }
-
-    /**
-     * @return the referencePose
-     */
-    public Pose3d getReferencePose() {
-        return referencePose;
-    }
-
-    /**
-     * Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
-     *
-     * @param referencePose the referencePose to set
-     */
-    public void setReferencePose(Pose3d referencePose) {
-        this.referencePose = referencePose;
-    }
-
-    /**
-     * Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
-     *
-     * @param referencePose the referencePose to set
-     */
-    public void setReferencePose(Pose2d referencePose) {
-        setReferencePose(new Pose3d(referencePose));
-    }
-
-    /**
-     * Update the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE
-     *
-     * @param lastPose the lastPose to set
-     */
-    public void setLastPose(Pose3d lastPose) {
-        this.lastPose = lastPose;
-    }
-
-    private void reportFiducialPoseError(int fiducialId) {
-        if (!reportedErrors.contains(fiducialId)) {
-            DriverStation.reportError(
-                    "[RobotPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false);
-            reportedErrors.add(fiducialId);
-        }
-    }
-}
diff --git a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java
deleted file mode 100644
index 89adf94bcf..0000000000
--- a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java
+++ /dev/null
@@ -1,93 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-package org.photonvision.estimation;
-
-import edu.wpi.first.math.geometry.Transform3d;
-
-/**
- * The best estimated transformation from solvePnP, and possibly an alternate transformation
- * depending on the solvePNP method. If an alternate solution is present, the ambiguity value
- * represents the ratio of reprojection error in the best solution to the alternate (best /
- * alternate).
- *
- * <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
- * method.
- */
-public class PNPResults {
-    /**
-     * If this result is valid. A false value indicates there was an error in estimation, and this
-     * result should not be used.
-     */
-    public final boolean isPresent;
-
-    /**
-     * The best-fit transform. The coordinate frame of this transform depends on the method which gave
-     * this result.
-     */
-    public final Transform3d best;
-
-    /** Reprojection error of the best solution, in pixels */
-    public final double bestReprojErr;
-
-    /**
-     * Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
-     * to the best solution.
-     */
-    public final Transform3d alt;
-
-    /** If no alternate solution is found, this is bestReprojErr */
-    public final double altReprojErr;
-
-    /** If no alternate solution is found, this is 0 */
-    public final double ambiguity;
-
-    /** An empty (invalid) result. */
-    public PNPResults() {
-        this.isPresent = false;
-        this.best = new Transform3d();
-        this.alt = new Transform3d();
-        this.ambiguity = 0;
-        this.bestReprojErr = 0;
-        this.altReprojErr = 0;
-    }
-
-    public PNPResults(Transform3d best, double bestReprojErr) {
-        this(best, best, 0, bestReprojErr, bestReprojErr);
-    }
-
-    public PNPResults(
-            Transform3d best,
-            Transform3d alt,
-            double ambiguity,
-            double bestReprojErr,
-            double altReprojErr) {
-        this.isPresent = true;
-        this.best = best;
-        this.alt = alt;
-        this.ambiguity = ambiguity;
-        this.bestReprojErr = bestReprojErr;
-        this.altReprojErr = altReprojErr;
-    }
-}
diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
index e50588c640..1c9ef2ce1e 100644
--- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
+++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
@@ -50,9 +50,9 @@
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.networktables.NTTopicSet;
 import org.photonvision.estimation.OpenCVHelp;
-import org.photonvision.estimation.PNPResults;
 import org.photonvision.estimation.RotTrlTransform3d;
 import org.photonvision.estimation.TargetModel;
+import org.photonvision.targeting.PNPResults;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java
index 9f301486a6..58e3e2fcf7 100644
--- a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java
+++ b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java
@@ -35,6 +35,7 @@
 import org.photonvision.PhotonTargetSortMode;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.networktables.NTTopicSet;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
@@ -140,7 +141,8 @@ public void submitProcessedFrame(
             targetList.sort(sortMode.getComparator());
         }
 
-        PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList);
+        PhotonPipelineResult newResult =
+                new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults());
         var newPacket = new Packet(newResult.getPacketSize());
         newResult.populatePacket(newPacket);
         ts.rawBytesEntry.set(newPacket.getData());
diff --git a/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp
new file mode 100644
index 0000000000..91d6ffd6c3
--- /dev/null
+++ b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp
@@ -0,0 +1,113 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 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/MultiTargetPNPResult.h"
+
+namespace photonlib {
+
+Packet& operator<<(Packet& packet, const MultiTargetPnpResult& target) {
+  packet << target.result;
+
+  size_t i;
+  for (i = 0; i < target.fiducialIdsUsed.capacity(); i++) {
+    if (i < target.fiducialIdsUsed.size()) {
+      packet << static_cast<int16_t>(target.fiducialIdsUsed[i]);
+    } else {
+      packet << static_cast<int16_t>(-1);
+    }
+  }
+
+  return packet;
+}
+
+Packet& operator>>(Packet& packet, MultiTargetPnpResult& target) {
+  packet >> target.result;
+
+  target.fiducialIdsUsed.clear();
+  for (size_t i = 0; i < target.fiducialIdsUsed.capacity(); i++) {
+    int16_t id = 0;
+    packet >> id;
+
+    if (id > -1) {
+      target.fiducialIdsUsed.push_back(id);
+    }
+  }
+
+  return packet;
+}
+
+// Encode a transform3d
+Packet& operator<<(Packet& packet, const frc::Transform3d& transform) {
+  packet << transform.Translation().X().value()
+         << transform.Translation().Y().value()
+         << transform.Translation().Z().value()
+         << transform.Rotation().GetQuaternion().W()
+         << transform.Rotation().GetQuaternion().X()
+         << transform.Rotation().GetQuaternion().Y()
+         << transform.Rotation().GetQuaternion().Z();
+
+  return packet;
+}
+
+// Decode a transform3d
+Packet& operator>>(Packet& packet, frc::Transform3d& transform) {
+  frc::Transform3d ret;
+
+  // We use these for best and alt transforms below
+  double x = 0;
+  double y = 0;
+  double z = 0;
+  double w = 0;
+
+  // decode and unitify translation
+  packet >> x >> y >> z;
+  const auto translation = frc::Translation3d(
+      units::meter_t(x), units::meter_t(y), units::meter_t(z));
+
+  // decode and add units to rotation
+  packet >> w >> x >> y >> z;
+  const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+
+  transform = frc::Transform3d(translation, rotation);
+
+  return packet;
+}
+
+Packet& operator<<(Packet& packet, PNPResults const& result) {
+  packet << result.isValid << result.best << result.alt
+         << result.bestReprojectionErr << result.altReprojectionErr
+         << result.ambiguity;
+
+  return packet;
+}
+
+Packet& operator>>(Packet& packet, PNPResults& result) {
+  packet >> result.isValid >> result.best >> result.alt >>
+      result.bestReprojectionErr >> result.altReprojectionErr >>
+      result.ambiguity;
+
+  return packet;
+}
+
+}  // namespace photonlib
diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp
index 9f6d0a8218..462dd25a90 100644
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp
@@ -40,7 +40,7 @@ bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const {
 
 Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
   // Encode latency and number of targets.
-  packet << result.latency.value() * 1000
+  packet << result.latency.value() * 1000 << result.m_pnpResults
          << static_cast<int8_t>(result.targets.size());
 
   // Encode the information of each target.
@@ -52,9 +52,9 @@ Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
 
 Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
   // Decode latency, existence of targets, and number of targets.
-  int8_t targetCount = 0;
   double latencyMillis = 0;
-  packet >> latencyMillis >> targetCount;
+  int8_t targetCount = 0;
+  packet >> latencyMillis >> result.m_pnpResults >> targetCount;
   result.latency = units::second_t(latencyMillis / 1000.0);
 
   result.targets.clear();
diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
index f25e01eba5..080243749f 100644
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
@@ -82,7 +82,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
       poseCacheTimestamp(-1_s) {}
 
 void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
-  if (strategy == MULTI_TAG_PNP) {
+  if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR ||
+      strategy == MULTI_TAG_PNP_ON_RIO) {
     FRC_ReportError(
         frc::warn::Warning,
         "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
@@ -162,8 +163,12 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     case AVERAGE_BEST_TARGETS:
       ret = AverageBestTargetsStrategy(result);
       break;
-    case ::photonlib::MULTI_TAG_PNP:
-      ret = MultiTagPnpStrategy(result, cameraMatrixData, cameraDistCoeffs);
+    case MULTI_TAG_PNP_ON_COPROCESSOR:
+      ret =
+          MultiTagOnCoprocStrategy(result, cameraMatrixData, cameraDistCoeffs);
+      break;
+    case MULTI_TAG_PNP_ON_RIO:
+      ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
       break;
     default:
       FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -205,7 +210,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
       fiducialPose.value()
           .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
           .TransformBy(m_robotToCamera.Inverse()),
-      result.GetTimestamp(), result.GetTargets()};
+      result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
 }
 
 std::optional<EstimatedRobotPose>
@@ -241,14 +246,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
       pose = EstimatedRobotPose{
           targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
               .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp(), result.GetTargets()};
+          result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
     }
     if (bestDifference < smallestHeightDifference) {
       smallestHeightDifference = bestDifference;
       pose = EstimatedRobotPose{
           targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
               .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp(), result.GetTargets()};
+          result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
     }
   }
 
@@ -299,7 +304,8 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
     }
   }
 
-  return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets()};
+  return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets(),
+                            CLOSEST_TO_REFERENCE_POSE};
 }
 
 std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
@@ -351,7 +357,24 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
                 Rotation3d(rv));
 }
 
-std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
+    PhotonPipelineResult result, std::optional<cv::Mat> camMat,
+    std::optional<cv::Mat> distCoeffs) {
+  if (result.MultiTagResult().result.isValid) {
+    const auto field2camera = result.MultiTagResult().result.best;
+
+    const auto fieldToRobot =
+        frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
+    return photonlib::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
+                                         result.GetTargets(),
+                                         MULTI_TAG_PNP_ON_COPROCESSOR);
+  }
+
+  return Update(result, std::nullopt, std::nullopt,
+                this->multiTagFallbackStrategy);
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
     PhotonPipelineResult result, std::optional<cv::Mat> camMat,
     std::optional<cv::Mat> distCoeffs) {
   using namespace frc;
@@ -404,7 +427,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
 
   return photonlib::EstimatedRobotPose(
       pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
-      result.GetTargets());
+      result.GetTargets(), MULTI_TAG_PNP_ON_RIO);
 }
 
 std::optional<EstimatedRobotPose>
@@ -430,7 +453,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
       return EstimatedRobotPose{
           targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
               .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp(), result.GetTargets()};
+          result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS};
     }
     totalAmbiguity += 1. / target.GetPoseAmbiguity();
 
@@ -450,6 +473,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
   }
 
   return EstimatedRobotPose{frc::Pose3d(transform, rotation),
-                            result.GetTimestamp(), result.GetTargets()};
+                            result.GetTimestamp(), result.GetTargets(),
+                            AVERAGE_BEST_TARGETS};
 }
 }  // namespace photonlib
diff --git a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
deleted file mode 100644
index 4d25a0a482..0000000000
--- a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
+++ /dev/null
@@ -1,282 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 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/RobotPoseEstimator.h"
-
-#include <iostream>
-#include <limits>
-#include <map>
-#include <span>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include <frc/Errors.h>
-#include <frc/apriltag/AprilTagFieldLayout.h>
-#include <frc/geometry/Pose3d.h>
-#include <frc/geometry/Rotation3d.h>
-#include <frc/geometry/Transform3d.h>
-#include <units/time.h>
-
-#include "photonlib/PhotonCamera.h"
-#include "photonlib/PhotonPipelineResult.h"
-#include "photonlib/PhotonTrackedTarget.h"
-
-namespace photonlib {
-RobotPoseEstimator::RobotPoseEstimator(
-    std::shared_ptr<frc::AprilTagFieldLayout> tags, PoseStrategy strat,
-    std::vector<std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>>
-        cams)
-    : aprilTags(tags),
-      strategy(strat),
-      cameras(std::move(cams)),
-      lastPose(frc::Pose3d()),
-      referencePose(frc::Pose3d()) {}
-
-std::pair<frc::Pose3d, units::second_t> RobotPoseEstimator::Update() {
-  if (cameras.empty()) {
-    return std::make_pair(lastPose, units::second_t(0));
-  }
-
-  std::pair<frc::Pose3d, units::second_t> pair;
-  switch (strategy) {
-    case LOWEST_AMBIGUITY:
-      pair = LowestAmbiguityStrategy();
-      lastPose = pair.first;
-      return pair;
-    case CLOSEST_TO_CAMERA_HEIGHT:
-      pair = ClosestToCameraHeightStrategy();
-      lastPose = pair.first;
-      return pair;
-    case CLOSEST_TO_REFERENCE_POSE:
-      pair = ClosestToReferencePoseStrategy();
-      lastPose = pair.first;
-      return pair;
-    case CLOSEST_TO_LAST_POSE:
-      SetReferencePose(lastPose);
-      pair = ClosestToReferencePoseStrategy();
-      lastPose = pair.first;
-      return pair;
-    case AVERAGE_BEST_TARGETS:
-      pair = AverageBestTargetsStrategy();
-      lastPose = pair.first;
-      return pair;
-    default:
-      FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
-                      "");
-  }
-
-  return std::make_pair(lastPose, units::second_t(0));
-}
-
-std::pair<frc::Pose3d, units::second_t>
-RobotPoseEstimator::LowestAmbiguityStrategy() {
-  int lowestAI = -1;
-  int lowestAJ = -1;
-  double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
-  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
-    std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
-    std::span<const PhotonTrackedTarget> targets =
-        p.first->GetLatestResult().GetTargets();
-    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-      if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) {
-        lowestAI = i;
-        lowestAJ = j;
-        lowestAmbiguityScore = targets[j].GetPoseAmbiguity();
-      }
-    }
-  }
-
-  if (lowestAI == -1 || lowestAJ == -1) {
-    return std::make_pair(lastPose, units::second_t(0));
-  }
-
-  PhotonTrackedTarget bestTarget =
-      cameras[lowestAI].first->GetLatestResult().GetTargets()[lowestAJ];
-
-  std::optional<frc::Pose3d> fiducialPose =
-      aprilTags->GetTagPose(bestTarget.GetFiducialId());
-  if (!fiducialPose) {
-    FRC_ReportError(frc::warn::Warning,
-                    "Tried to get pose of unknown April Tag: {}",
-                    bestTarget.GetFiducialId());
-    return std::make_pair(lastPose, units::second_t(0));
-  }
-
-  return std::make_pair(
-      fiducialPose.value()
-          .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
-          .TransformBy(cameras[lowestAI].second.Inverse()),
-      cameras[lowestAI].first->GetLatestResult().GetTimestamp());
-}
-
-std::pair<frc::Pose3d, units::second_t>
-RobotPoseEstimator::ClosestToCameraHeightStrategy() {
-  units::meter_t smallestHeightDifference =
-      units::meter_t(std::numeric_limits<double>::infinity());
-  units::second_t stateTimestamp = units::second_t(0);
-  frc::Pose3d pose = lastPose;
-
-  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
-    std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
-    std::span<const PhotonTrackedTarget> targets =
-        p.first->GetLatestResult().GetTargets();
-    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-      PhotonTrackedTarget target = targets[j];
-      std::optional<frc::Pose3d> fiducialPose =
-          aprilTags->GetTagPose(target.GetFiducialId());
-      if (!fiducialPose) {
-        FRC_ReportError(frc::warn::Warning,
-                        "Tried to get pose of unknown April Tag: {}",
-                        target.GetFiducialId());
-        continue;
-      }
-      frc::Pose3d targetPose = fiducialPose.value();
-      units::meter_t alternativeDifference = units::math::abs(
-          p.second.Z() -
-          targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
-              .Z());
-      units::meter_t bestDifference = units::math::abs(
-          p.second.Z() -
-          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
-      if (alternativeDifference < smallestHeightDifference) {
-        smallestHeightDifference = alternativeDifference;
-        pose = targetPose.TransformBy(
-            target.GetAlternateCameraToTarget().Inverse());
-        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
-      }
-      if (bestDifference < smallestHeightDifference) {
-        smallestHeightDifference = bestDifference;
-        pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
-        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
-      }
-    }
-  }
-  return std::make_pair(pose, stateTimestamp);
-}
-
-std::pair<frc::Pose3d, units::second_t>
-RobotPoseEstimator::ClosestToReferencePoseStrategy() {
-  units::meter_t smallestDifference =
-      units::meter_t(std::numeric_limits<double>::infinity());
-  units::second_t stateTimestamp = units::second_t(0);
-  frc::Pose3d pose = lastPose;
-
-  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
-    std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
-    std::span<const PhotonTrackedTarget> targets =
-        p.first->GetLatestResult().GetTargets();
-    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-      PhotonTrackedTarget target = targets[j];
-      std::optional<frc::Pose3d> fiducialPose =
-          aprilTags->GetTagPose(target.GetFiducialId());
-      if (!fiducialPose) {
-        FRC_ReportError(frc::warn::Warning,
-                        "Tried to get pose of unknown April Tag: {}",
-                        target.GetFiducialId());
-        continue;
-      }
-      frc::Pose3d targetPose = fiducialPose.value();
-      units::meter_t alternativeDifference =
-          units::math::abs(referencePose.Translation().Distance(
-              targetPose
-                  .TransformBy(target.GetAlternateCameraToTarget().Inverse())
-                  .Translation()));
-      units::meter_t bestDifference =
-          units::math::abs(referencePose.Translation().Distance(
-              targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
-                  .Translation()));
-      if (alternativeDifference < smallestDifference) {
-        smallestDifference = alternativeDifference;
-        pose = targetPose.TransformBy(
-            target.GetAlternateCameraToTarget().Inverse());
-        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
-      }
-
-      if (bestDifference < smallestDifference) {
-        smallestDifference = bestDifference;
-        pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
-        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
-      }
-    }
-  }
-
-  return std::make_pair(pose, stateTimestamp);
-}
-
-std::pair<frc::Pose3d, units::second_t>
-RobotPoseEstimator::AverageBestTargetsStrategy() {
-  std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
-      tempPoses;
-  double totalAmbiguity = 0;
-  units::second_t timstampSum = units::second_t(0);
-
-  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
-    std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
-    std::span<const PhotonTrackedTarget> targets =
-        p.first->GetLatestResult().GetTargets();
-    timstampSum += p.first->GetLatestResult().GetTimestamp();
-    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-      PhotonTrackedTarget target = targets[j];
-      std::optional<frc::Pose3d> fiducialPose =
-          aprilTags->GetTagPose(target.GetFiducialId());
-      if (!fiducialPose) {
-        FRC_ReportError(frc::warn::Warning,
-                        "Tried to get pose of unknown April Tag: {}",
-                        target.GetFiducialId());
-        continue;
-      }
-
-      frc::Pose3d targetPose = fiducialPose.value();
-      if (target.GetPoseAmbiguity() == 0) {
-        FRC_ReportError(frc::warn::Warning,
-                        "Pose ambiguity of zero exists, using that instead!",
-                        "");
-        return std::make_pair(
-            targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
-            p.first->GetLatestResult().GetLatency() / 1000.);
-      }
-      totalAmbiguity += 1. / target.GetPoseAmbiguity();
-
-      tempPoses.push_back(std::make_pair(
-          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
-          std::make_pair(target.GetPoseAmbiguity(),
-                         p.first->GetLatestResult().GetTimestamp())));
-    }
-  }
-
-  frc::Translation3d transform = frc::Translation3d();
-  frc::Rotation3d rotation = frc::Rotation3d();
-
-  for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
-       tempPoses) {
-    double weight = (1. / pair.second.first) / totalAmbiguity;
-    transform = transform + pair.first.Translation() * weight;
-    rotation = rotation + pair.first.Rotation() * weight;
-  }
-
-  return std::make_pair(frc::Pose3d(transform, rotation),
-                        timstampSum / cameras.size());
-}
-}  // namespace photonlib
diff --git a/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h
new file mode 100644
index 0000000000..96681a0307
--- /dev/null
+++ b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h
@@ -0,0 +1,61 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 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/Transform3d.h>
+#include <wpi/SmallVector.h>
+
+#include "photonlib/Packet.h"
+
+namespace photonlib {
+
+class PNPResults {
+ public:
+  // This could be wrapped in an std::optional, but chose to do it this way to
+  // mirror Java
+  bool isValid;
+
+  frc::Transform3d best;
+  double bestReprojectionErr;
+
+  frc::Transform3d alt;
+  double altReprojectionErr;
+
+  double ambiguity;
+
+  friend Packet& operator<<(Packet& packet, const PNPResults& result);
+  friend Packet& operator>>(Packet& packet, PNPResults& result);
+};
+
+class MultiTargetPnpResult {
+ public:
+  PNPResults result;
+  wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
+
+  friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result);
+  friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result);
+};
+
+}  // namespace photonlib
diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h
index e6f6c4d49b..8370702b3b 100644
--- a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h
+++ b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h
@@ -31,6 +31,7 @@
 #include <units/time.h>
 #include <wpi/SmallVector.h>
 
+#include "photonlib/MultiTargetPNPResult.h"
 #include "photonlib/Packet.h"
 #include "photonlib/PhotonTrackedTarget.h"
 
@@ -87,6 +88,13 @@ class PhotonPipelineResult {
    */
   units::second_t GetTimestamp() const { return timestamp; }
 
+  /**
+   * Return the latest mulit-target result, as calculated on your coprocessor.
+   * Be sure to check getMultiTagResult().estimatedPose.isValid before using the
+   * pose estimate!
+   */
+  const MultiTargetPnpResult& MultiTagResult() const { return m_pnpResults; }
+
   /**
    * Sets the timestamp in seconds
    * @param timestamp The timestamp in seconds
@@ -119,6 +127,7 @@ class PhotonPipelineResult {
   units::second_t latency = 0_s;
   units::second_t timestamp = -1_s;
   wpi::SmallVector<PhotonTrackedTarget, 10> targets;
+  MultiTargetPnpResult m_pnpResults;
   inline static bool HAS_WARNED = false;
 };
 }  // namespace photonlib
diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
index 21f6a343cb..7c93ac9ce9 100644
--- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
+++ b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
@@ -44,7 +44,8 @@ enum PoseStrategy {
   CLOSEST_TO_REFERENCE_POSE,
   CLOSEST_TO_LAST_POSE,
   AVERAGE_BEST_TARGETS,
-  MULTI_TAG_PNP
+  MULTI_TAG_PNP_ON_COPROCESSOR,
+  MULTI_TAG_PNP_ON_RIO,
 };
 
 struct EstimatedRobotPose {
@@ -57,11 +58,16 @@ struct EstimatedRobotPose {
   /** A list of the targets used to compute this pose */
   wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
 
+  /** The strategy actually used to produce this pose */
+  PoseStrategy strategy;
+
   EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
-                     std::span<const PhotonTrackedTarget> targets)
+                     std::span<const PhotonTrackedTarget> targets,
+                     PoseStrategy strategy_)
       : estimatedPose(pose_),
         timestamp(time_),
-        targetsUsed(targets.data(), targets.data() + targets.size()) {}
+        targetsUsed(targets.data(), targets.data() + targets.size()),
+        strategy(strategy_) {}
 };
 
 /**
@@ -260,14 +266,23 @@ class PhotonPoseEstimator {
   std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
       PhotonPipelineResult result);
 
+  /**
+   * Return the pose calculated by combining all tags into one on coprocessor
+   *
+   * @return the estimated position of the robot in the FCS
+   */
+  std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
+      PhotonPipelineResult result, std::optional<cv::Mat> camMat,
+      std::optional<cv::Mat> distCoeffs);
+
   /**
    * Return the pose calculation using all targets in view in the same PNP()
    calculation
-
+   *
    * @return the estimated position of the robot in the FCS and the estimated
    timestamp of this estimation.
    */
-  std::optional<EstimatedRobotPose> MultiTagPnpStrategy(
+  std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
       PhotonPipelineResult result, std::optional<cv::Mat> camMat,
       std::optional<cv::Mat> distCoeffs);
 
diff --git a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h
deleted file mode 100644
index c9282b5920..0000000000
--- a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h
+++ /dev/null
@@ -1,187 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 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 <map>
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include <frc/geometry/Pose3d.h>
-#include <frc/geometry/Transform3d.h>
-
-#include "photonlib/PhotonCamera.h"
-
-namespace frc {
-class AprilTagFieldLayout;
-}  // namespace frc
-
-namespace photonlib {
-enum PoseStrategy : int {
-  LOWEST_AMBIGUITY,
-  CLOSEST_TO_CAMERA_HEIGHT,
-  CLOSEST_TO_REFERENCE_POSE,
-  CLOSEST_TO_LAST_POSE,
-  AVERAGE_BEST_TARGETS
-};
-
-/**
- * The RobotPoseEstimator class filters or combines readings from all the
- * fiducials visible at a given timestamp on the field to produce a single robot
- * in field pose, using the strategy set below. Example usage can be found in
- * our apriltagExample example project.
- */
-class RobotPoseEstimator {
- public:
-  using map_value_type =
-      std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
-  using size_type = std::vector<map_value_type>::size_type;
-
-  /**
-   * Create a new RobotPoseEstimator.
-   *
-   * <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
-   * <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
-   * at (1.0,2.0,3.0) </code> }
-   *
-   * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
-   * respect to the FIRST field.
-   * @param strategy The strategy it should use to determine the best pose.
-   * @param cameras An ArrayList of Pairs of PhotonCameras and their respective
-   * Transform3ds from the center of the robot to the cameras.
-   */
-  explicit RobotPoseEstimator(
-      std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
-      PoseStrategy strategy, std::vector<map_value_type> cameras);
-
-  /**
-   * Get the AprilTagFieldLayout being used by the PositionEstimator.
-   *
-   * @return the AprilTagFieldLayout
-   */
-  std::shared_ptr<frc::AprilTagFieldLayout> getFieldLayout() const {
-    return aprilTags;
-  }
-
-  /**
-   * Set the cameras to be used by the PoseEstimator.
-   *
-   * @param cameras cameras to set.
-   */
-  inline void SetCameras(
-      const std::vector<std::pair<std::shared_ptr<PhotonCamera>,
-                                  frc::Transform3d>>& cameras) {
-    this->cameras = cameras;
-  }
-
-  /**
-   * Get the Position Estimation Strategy being used by the Position Estimator.
-   *
-   * @return the strategy
-   */
-  PoseStrategy GetPoseStrategy() const { return strategy; }
-
-  /**
-   * Set the Position Estimation Strategy used by the Position Estimator.
-   *
-   * @param strategy the strategy to set
-   */
-  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
-
-  /**
-   * Return the reference position that is being used by the estimator.
-   *
-   * @return the referencePose
-   */
-  frc::Pose3d GetReferencePose() const { return referencePose; }
-
-  /**
-   * Update the stored reference pose for use when using the
-   * CLOSEST_TO_REFERENCE_POSE strategy.
-   *
-   * @param referencePose the referencePose to set
-   */
-  inline void SetReferencePose(frc::Pose3d referencePose) {
-    this->referencePose = referencePose;
-  }
-
-  /**
-   * Update the stored last pose. Useful for setting the initial estimate when
-   * using the CLOSEST_TO_LAST_POSE strategy.
-   *
-   * @param lastPose the lastPose to set
-   */
-  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
-
-  std::pair<frc::Pose3d, units::second_t> Update();
-
- private:
-  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
-  PoseStrategy strategy;
-  std::vector<map_value_type> cameras;
-  frc::Pose3d lastPose;
-  frc::Pose3d referencePose;
-
-  /**
-   * Return the estimated position of the robot with the lowest position
-   * ambiguity from a List of pipeline results.
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::pair<frc::Pose3d, units::second_t> LowestAmbiguityStrategy();
-
-  /**
-   * Return the estimated position of the robot using the target with the lowest
-   * delta height difference between the estimated and actual height of the
-   * camera.
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::pair<frc::Pose3d, units::second_t> ClosestToCameraHeightStrategy();
-
-  /**
-   * Return the estimated position of the robot using the target with the lowest
-   * delta in the vector magnitude between it and the reference pose.
-   *
-   * @param referencePose reference pose to check vector magnitude difference
-   * against.
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::pair<frc::Pose3d, units::second_t> ClosestToReferencePoseStrategy();
-
-  /**
-   * Return the average of the best target poses using ambiguity as weight.
-
-   * @return the estimated position of the robot in the FCS and the estimated
-   timestamp of this
-   *     estimation.
-   */
-  std::pair<frc::Pose3d, units::second_t> AverageBestTargetsStrategy();
-};
-
-}  // namespace photonlib
diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java
index 4f84afb720..c29a629b5b 100644
--- a/photon-lib/src/test/java/org/photonvision/PacketTest.java
+++ b/photon-lib/src/test/java/org/photonvision/PacketTest.java
@@ -30,6 +30,8 @@
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.dataflow.structures.Packet;
+import org.photonvision.targeting.MultiTargetPNPResults;
+import org.photonvision.targeting.PNPResults;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
@@ -127,4 +129,62 @@ void testSimplePipelineResult() {
 
         Assertions.assertEquals(result2, b2);
     }
+
+    @Test
+    public void testMultiTargetSerde() {
+        var result =
+                new PhotonPipelineResult(
+                        2,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        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),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        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),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))),
+                        new MultiTargetPNPResults(
+                                new PNPResults(
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1),
+                                List.of(1, 2, 3)));
+
+        Packet packet = new Packet(result.getPacketSize());
+        result.populatePacket(packet);
+
+        var result_deserialized = new PhotonPipelineResult();
+        result_deserialized.createFromPacket(packet);
+
+        Assertions.assertEquals(result, result_deserialized);
+    }
 }
diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
index 5ab7d00ce2..c53e006717 100644
--- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
+++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
@@ -297,12 +297,12 @@ public void testYawAngles(double testYaw) {
         var res = camera.getLatestResult();
         assertTrue(res.hasTargets());
         var tgt = res.getBestTarget();
-        assertEquals(tgt.getYaw(), testYaw, kRotDeltaDeg);
+        assertEquals(testYaw, tgt.getYaw(), kRotDeltaDeg);
     }
 
     @ParameterizedTest
     @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999})
-    public void testCameraPitch(double testPitch) {
+    public void testPitchAngles(double testPitch) {
         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));
@@ -331,7 +331,7 @@ public void testCameraPitch(double testPitch) {
         assertEquals(testPitch, tgt.getPitch(), kRotDeltaDeg);
     }
 
-    private static Stream<Arguments> distCalCParamProvider() {
+    private static Stream<Arguments> testDistanceCalcArgs() {
         // Arbitrary and fairly random assortment of distances, camera pitches, and heights
         return Stream.of(
                 Arguments.of(5, -15.98, 0),
@@ -354,7 +354,7 @@ private static Stream<Arguments> distCalCParamProvider() {
     }
 
     @ParameterizedTest
-    @MethodSource("distCalCParamProvider")
+    @MethodSource("testDistanceCalcArgs")
     public void testDistanceCalc(double testDist, double testPitch, double testHeight) {
         // Assume dist along ground and tgt height the same. Iterate over other parameters.
 
diff --git a/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java b/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java
index 5ead3be618..fa70467c5f 100644
--- a/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java
+++ b/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java
@@ -84,7 +84,10 @@ public void testMeme() throws IOException, InterruptedException {
 
         var pe =
                 new PhotonPoseEstimator(
-                        tagLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, cam, robotToCamera);
+                        tagLayout,
+                        PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+                        cam,
+                        robotToCamera);
 
         var field = new Field2d();
         SmartDashboard.putData(field);
diff --git a/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp
deleted file mode 100644
index 09a2bdf461..0000000000
--- a/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp
+++ /dev/null
@@ -1,442 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 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 <map>
-#include <utility>
-#include <vector>
-
-#include <frc/apriltag/AprilTagFieldLayout.h>
-#include <frc/geometry/Pose3d.h>
-#include <frc/geometry/Rotation3d.h>
-#include <frc/geometry/Transform3d.h>
-#include <units/angle.h>
-#include <units/length.h>
-#include <wpi/SmallVector.h>
-
-#include "gtest/gtest.h"
-#include "photonlib/PhotonCamera.h"
-#include "photonlib/PhotonPipelineResult.h"
-#include "photonlib/PhotonTrackedTarget.h"
-#include "photonlib/RobotPoseEstimator.h"
-
-static wpi::SmallVector<std::pair<double, double>, 4> corners{
-    std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
-static std::vector<std::pair<double, double>> detectedCorners{
-    std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
-
-TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
-  std::vector<frc::AprilTag> tags = {
-      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
-                      frc::Rotation3d())},
-      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
-                      frc::Rotation3d())}};
-
-  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
-      std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
-
-  std::vector<
-      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
-      cameras;
-
-  std::shared_ptr<photonlib::PhotonCamera> cameraOne =
-      std::make_shared<photonlib::PhotonCamera>("test");
-  std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
-      std::make_shared<photonlib::PhotonCamera>("test");
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.0, 4.0, 0,
-          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
-                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
-                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-          0.7, corners, detectedCorners},
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.1, 6.7, 1,
-          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3, corners, detectedCorners}};
-
-  cameraOne->test = true;
-  cameraOne->testResult = {2_s, targets};
-  cameraOne->testResult.SetTimestamp(units::second_t(11));
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
-      photonlib::PhotonTrackedTarget{
-          9.0, -2.0, 19.0, 3.0, 0,
-          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
-                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
-                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-          0.4, corners, detectedCorners}};
-
-  cameraTwo->test = true;
-  cameraTwo->testResult = {4_s, targetsTwo};
-  cameraTwo->testResult.SetTimestamp(units::second_t(units::second_t(16)));
-
-  cameras.push_back(std::make_pair(
-      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  cameras.push_back(std::make_pair(
-      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  photonlib::RobotPoseEstimator estimator(aprilTags,
-                                          photonlib::LOWEST_AMBIGUITY, cameras);
-  std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
-      estimator.Update();
-  frc::Pose3d pose = estimatedPose.first;
-
-  EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
-  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
-  EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
-  EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
-}
-
-TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
-  std::vector<frc::AprilTag> tags = {
-      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
-                      frc::Rotation3d())},
-      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
-                      frc::Rotation3d())},
-  };
-  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
-      std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
-
-  std::vector<
-      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
-      cameras;
-
-  std::shared_ptr<photonlib::PhotonCamera> cameraOne =
-      std::make_shared<photonlib::PhotonCamera>("test");
-  std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
-      std::make_shared<photonlib::PhotonCamera>("test");
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.0, 4.0, 1,
-          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7, corners, detectedCorners},
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.1, 6.7, 1,
-          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3, corners, detectedCorners}};
-
-  cameraOne->test = true;
-  cameraOne->testResult = {2_s, targets};
-  cameraOne->testResult.SetTimestamp(units::second_t(4));
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
-      photonlib::PhotonTrackedTarget{
-          9.0, -2.0, 19.0, 3.0, 0,
-          frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4, corners, detectedCorners}};
-
-  cameraTwo->test = true;
-  cameraTwo->testResult = {4_s, targetsTwo};
-  cameraOne->testResult.SetTimestamp(units::second_t(12));
-
-  cameras.push_back(std::make_pair(
-      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 4_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  cameras.push_back(std::make_pair(
-      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 2_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  photonlib::RobotPoseEstimator estimator(
-      aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, cameras);
-  std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
-      estimator.Update();
-  frc::Pose3d pose = estimatedPose.first;
-
-  EXPECT_NEAR(12, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
-  EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
-  EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
-  EXPECT_NEAR(4, units::unit_cast<double>(pose.Z()), .01);
-}
-
-TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
-  std::vector<frc::AprilTag> tags = {
-      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
-                      frc::Rotation3d())},
-      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
-                      frc::Rotation3d())},
-  };
-  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
-      std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
-
-  std::vector<
-      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
-      cameras;
-
-  std::shared_ptr<photonlib::PhotonCamera> cameraOne =
-      std::make_shared<photonlib::PhotonCamera>("test");
-  std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
-      std::make_shared<photonlib::PhotonCamera>("test");
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.0, 4.0, 1,
-          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7, corners, detectedCorners},
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.1, 6.7, 1,
-          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3, corners, detectedCorners}};
-
-  cameraOne->test = true;
-  cameraOne->testResult = {2_s, targets};
-  cameraOne->testResult.SetTimestamp(units::second_t(4));
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
-      photonlib::PhotonTrackedTarget{
-          9.0, -2.0, 19.0, 3.0, 0,
-          frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4, corners, detectedCorners}};
-
-  cameraTwo->test = true;
-  cameraTwo->testResult = {4_s, targetsTwo};
-  cameraTwo->testResult.SetTimestamp(units::second_t(17));
-
-  cameras.push_back(std::make_pair(
-      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  cameras.push_back(std::make_pair(
-      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  photonlib::RobotPoseEstimator estimator(
-      aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras);
-  estimator.SetReferencePose(
-      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
-  std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
-      estimator.Update();
-  frc::Pose3d pose = estimatedPose.first;
-
-  EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
-  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
-  EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
-  EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
-}
-
-TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
-  std::vector<frc::AprilTag> tags = {
-      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
-                      frc::Rotation3d())},
-      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
-                      frc::Rotation3d())}};
-  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
-      std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
-
-  std::vector<
-      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
-      cameras;
-
-  std::shared_ptr<photonlib::PhotonCamera> cameraOne =
-      std::make_shared<photonlib::PhotonCamera>("test");
-  std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
-      std::make_shared<photonlib::PhotonCamera>("test");
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.0, 4.0, 1,
-          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7, corners, detectedCorners},
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.1, 6.7, 1,
-          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3, corners, detectedCorners}};
-
-  cameraOne->test = true;
-  cameraOne->testResult = {2_s, targets};
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
-      photonlib::PhotonTrackedTarget{
-          9.0, -2.0, 19.0, 3.0, 0,
-          frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4, corners, detectedCorners}};
-
-  cameraTwo->test = true;
-  cameraTwo->testResult = {4_s, targetsTwo};
-
-  cameras.push_back(std::make_pair(
-      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  cameras.push_back(std::make_pair(
-      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  photonlib::RobotPoseEstimator estimator(
-      aprilTags, photonlib::CLOSEST_TO_LAST_POSE, cameras);
-  estimator.SetLastPose(
-      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
-  std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
-      estimator.Update();
-  frc::Pose3d pose = estimatedPose.first;
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targetsThree{
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.0, 4.0, 1,
-          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7, corners, detectedCorners},
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.1, 6.7, 0,
-          frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3, corners, detectedCorners}};
-
-  cameraOne->testResult = {2_s, targetsThree};
-  cameraOne->testResult.SetTimestamp(units::second_t(7));
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsFour{
-      photonlib::PhotonTrackedTarget{
-          9.0, -2.0, 19.0, 3.0, 0,
-          frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4, corners, detectedCorners}};
-
-  cameraTwo->testResult = {4_s, targetsFour};
-  cameraTwo->testResult.SetTimestamp(units::second_t(13));
-
-  std::vector<
-      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
-      camerasUpdated;
-  camerasUpdated.push_back(std::make_pair(
-      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  camerasUpdated.push_back(std::make_pair(
-      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  estimator.SetCameras(camerasUpdated);
-  estimatedPose = estimator.Update();
-  pose = estimatedPose.first;
-
-  EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
-              .01);
-  EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
-  EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
-  EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
-}
-
-TEST(RobotPoseEstimatorTest, AverageBestPoses) {
-  std::vector<frc::AprilTag> tags = {
-      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
-                      frc::Rotation3d())},
-      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
-                      frc::Rotation3d())}};
-  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
-      std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
-
-  std::vector<
-      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
-      cameras;
-
-  std::shared_ptr<photonlib::PhotonCamera> cameraOne =
-      std::make_shared<photonlib::PhotonCamera>("test");
-  std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
-      std::make_shared<photonlib::PhotonCamera>("test");
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.0, 4.0, 0,
-          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7, corners, detectedCorners},
-      photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.1, 6.7, 1,
-          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3, corners, detectedCorners}};
-
-  cameraOne->test = true;
-  cameraOne->testResult = {2_s, targets};
-  cameraOne->testResult.SetTimestamp(units::second_t(10));
-
-  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
-      photonlib::PhotonTrackedTarget{
-          9.0, -2.0, 19.0, 3.0, 0,
-          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
-                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4, corners, detectedCorners}};
-
-  cameraTwo->test = true;
-  cameraTwo->testResult = {4_s, targetsTwo};
-  cameraTwo->testResult.SetTimestamp(units::second_t(20));
-
-  cameras.push_back(std::make_pair(
-      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  cameras.push_back(std::make_pair(
-      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
-                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
-  photonlib::RobotPoseEstimator estimator(
-      aprilTags, photonlib::AVERAGE_BEST_TARGETS, cameras);
-  std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
-      estimator.Update();
-  frc::Pose3d pose = estimatedPose.first;
-
-  EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
-              .01);
-  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
-  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
-  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
-}
diff --git a/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java b/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java
index 19fc18f937..f7f0345723 100644
--- a/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java
+++ b/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java
@@ -265,6 +265,30 @@ public void onBinaryMessage(WsBinaryMessageContext context) {
                                 dcService.publishEvent(changePipelineEvent);
                                 break;
                             }
+                        case SMT_SAVEINPUTSNAPSHOT:
+                            {
+                                var takeInputSnapshotEvent =
+                                        new IncomingWebSocketEvent<>(
+                                                DataChangeDestination.DCD_ACTIVEMODULE,
+                                                "saveInputSnapshot",
+                                                0,
+                                                cameraIndex,
+                                                context);
+                                dcService.publishEvent(takeInputSnapshotEvent);
+                                break;
+                            }
+                        case SMT_SAVEOUTPUTSNAPSHOT:
+                            {
+                                var takeOutputSnapshotEvent =
+                                        new IncomingWebSocketEvent<>(
+                                                DataChangeDestination.DCD_ACTIVEMODULE,
+                                                "saveOutputSnapshot",
+                                                0,
+                                                cameraIndex,
+                                                context);
+                                dcService.publishEvent(takeOutputSnapshotEvent);
+                                break;
+                            }
                         case SMT_TAKECALIBRATIONSNAPSHOT:
                             {
                                 var takeCalSnapshotEvent =
diff --git a/photon-server/src/main/java/org/photonvision/server/DataSocketMessageType.java b/photon-server/src/main/java/org/photonvision/server/DataSocketMessageType.java
index 256695f2c8..f5222cf371 100644
--- a/photon-server/src/main/java/org/photonvision/server/DataSocketMessageType.java
+++ b/photon-server/src/main/java/org/photonvision/server/DataSocketMessageType.java
@@ -32,6 +32,8 @@ public enum DataSocketMessageType {
     SMT_PIPELINESETTINGCHANGE("changePipelineSetting"),
     SMT_CURRENTPIPELINE("currentPipeline"),
     SMT_STARTPNPCALIBRATION("startPnpCalibration"),
+    SMT_SAVEINPUTSNAPSHOT("saveInputSnapshot"),
+    SMT_SAVEOUTPUTSNAPSHOT("saveOutputSnapshot"),
     SMT_TAKECALIBRATIONSNAPSHOT("takeCalibrationSnapshot"),
     SMT_DUPLICATEPIPELINE("duplicatePipeline"),
     SMT_CHANGEBRIGHTNESS("enabledLEDPercentage"),
diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
index c255748195..9547c2cdf0 100644
--- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
+++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
@@ -246,6 +246,48 @@ public static void onNetworkConfigRequest(Context ctx) {
         }
     }
 
+    public static void onAprilTagFieldLayoutRequest(Context ctx) {
+        var file = ctx.uploadedFile("data");
+
+        if (file == null) {
+            ctx.status(400);
+            ctx.result(
+                    "No File was sent with the request. Make sure that the field layout json is sent at the key 'data'");
+            logger.error(
+                    "No File was sent with the request. Make sure that the field layout json is sent at the key 'data'");
+            return;
+        }
+
+        if (!file.extension().contains("json")) {
+            ctx.status(400);
+            ctx.result(
+                    "The uploaded file was not of type 'json'. The uploaded file should be a .json file.");
+            logger.error(
+                    "The uploaded file was not of type 'json'. The uploaded file should be a .json file.");
+            return;
+        }
+
+        // Create a temp file
+        var tempFilePath = handleTempFileCreation(file);
+
+        if (tempFilePath.isEmpty()) {
+            ctx.status(500);
+            ctx.result("There was an error while creating a temporary copy of the file");
+            logger.error("There was an error while creating a temporary copy of the file");
+            return;
+        }
+
+        if (ConfigManager.getInstance().saveUploadedAprilTagFieldLayout(tempFilePath.get().toPath())) {
+            ctx.status(200);
+            ctx.result("Successfully saved the uploaded AprilTagFieldLayout");
+            logger.info("Successfully saved the uploaded AprilTagFieldLayout");
+        } else {
+            ctx.status(500);
+            ctx.result("There was an error while saving the uploaded AprilTagFieldLayout");
+            logger.error("There was an error while saving the uploaded AprilTagFieldLayout");
+        }
+    }
+
     public static void onOfflineUpdateRequest(Context ctx) {
         var file = ctx.uploadedFile("jarData");
 
@@ -495,9 +537,12 @@ private static Optional<File> handleTempFileCreation(UploadedFile file) {
                 new File(Path.of(System.getProperty("java.io.tmpdir"), file.filename()).toString());
         boolean makeDirsRes = tempFilePath.getParentFile().mkdirs();
 
-        if (!makeDirsRes) {
+        if (!makeDirsRes && !(tempFilePath.getParentFile().exists())) {
             logger.error(
-                    "There was an error while uploading " + file.filename() + " to the temp folder!");
+                    "There was an error while creating "
+                            + tempFilePath.getAbsolutePath()
+                            + "! Exists: "
+                            + tempFilePath.getParentFile().exists());
             return Optional.empty();
         }
 
@@ -505,7 +550,10 @@ private static Optional<File> handleTempFileCreation(UploadedFile file) {
             FileUtils.copyInputStreamToFile(file.content(), tempFilePath);
         } catch (IOException e) {
             logger.error(
-                    "There was an error while uploading " + file.filename() + " to the temp folder!");
+                    "There was an error while copying "
+                            + file.filename()
+                            + " to the temp file "
+                            + tempFilePath.getAbsolutePath());
             return Optional.empty();
         }
 
diff --git a/photon-server/src/main/java/org/photonvision/server/Server.java b/photon-server/src/main/java/org/photonvision/server/Server.java
index 724459e14f..b6595d5b94 100644
--- a/photon-server/src/main/java/org/photonvision/server/Server.java
+++ b/photon-server/src/main/java/org/photonvision/server/Server.java
@@ -97,6 +97,7 @@ public static void start(int port) {
         app.post("/api/settings/hardwareConfig", RequestHandler::onHardwareConfigRequest);
         app.post("/api/settings/hardwareSettings", RequestHandler::onHardwareSettingsRequest);
         app.post("/api/settings/networkConfig", RequestHandler::onNetworkConfigRequest);
+        app.post("/api/settings/aprilTagFieldLayout", RequestHandler::onAprilTagFieldLayoutRequest);
         app.post("/api/settings/general", RequestHandler::onGeneralSettingsRequest);
         app.post("/api/settings/camera", RequestHandler::onCameraSettingsRequest);
         app.post("/api/settings/camera/setNickname", RequestHandler::onCameraNicknameChangeRequest);
diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle
index 168ecc981e..e4cc2d7d96 100644
--- a/photon-targeting/build.gradle
+++ b/photon-targeting/build.gradle
@@ -4,6 +4,7 @@ apply from: "${rootDir}/shared/common.gradle"
 
 dependencies {
     implementation wpilibTools.deps.wpilibJava("wpimath")
+    implementation wpilibTools.deps.wpilibJava("apriltag")
     implementation "org.ejml:ejml-simple:0.42"
 
     // Junit
diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
index 02a8f0774d..e97e3f1658 100644
--- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
+++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
@@ -85,6 +85,16 @@ public void encode(byte src) {
         packetData[writePos++] = src;
     }
 
+    /**
+     * Encodes the short into the packet.
+     *
+     * @param src The short to encode.
+     */
+    public void encode(short src) {
+        packetData[writePos++] = (byte) (src >>> 8);
+        packetData[writePos++] = (byte) src;
+    }
+
     /**
      * Encodes the integer into the packet.
      *
@@ -196,4 +206,11 @@ public double[] decodeDoubleArray(int len) {
         }
         return ret;
     }
+
+    public short decodeShort() {
+        if (packetData.length < readPos + 1) {
+            return 0;
+        }
+        return (short) ((0xff & packetData[readPos++]) << 8 | (0xff & packetData[readPos++]));
+    }
 }
diff --git a/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java b/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java
similarity index 62%
rename from photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java
rename to photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java
index 40232aa8b8..b0b915c93e 100644
--- a/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java
+++ b/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java
@@ -1,25 +1,18 @@
 /*
- * MIT License
+ * Copyright (C) Photon Vision.
  *
- * Copyright (c) PhotonVision
+ * 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.
  *
- * 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:
+ * 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.
  *
- * 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.
+ * 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.estimation;
diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
similarity index 95%
rename from photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java
rename to photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
index 67002101ce..3f89918d62 100644
--- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java
+++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
@@ -1,25 +1,18 @@
 /*
- * MIT License
+ * Copyright (C) Photon Vision.
  *
- * Copyright (c) PhotonVision
+ * 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.
  *
- * 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:
+ * 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.
  *
- * 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.
+ * 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.estimation;
@@ -52,6 +45,7 @@
 import org.opencv.core.Rect;
 import org.opencv.core.RotatedRect;
 import org.opencv.imgproc.Imgproc;
+import org.photonvision.targeting.PNPResults;
 import org.photonvision.targeting.TargetCorner;
 
 public final class OpenCVHelp {
diff --git a/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
similarity index 86%
rename from photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
rename to photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
index 0fac0334eb..2b79e920d6 100644
--- a/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
+++ b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
@@ -1,25 +1,18 @@
 /*
- * MIT License
+ * Copyright (C) Photon Vision.
  *
- * Copyright (c) PhotonVision
+ * 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.
  *
- * 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:
+ * 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.
  *
- * 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.
+ * 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.estimation;
diff --git a/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java
similarity index 86%
rename from photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java
rename to photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java
index 90044abb38..4a4c22449d 100644
--- a/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java
+++ b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java
@@ -1,25 +1,18 @@
 /*
- * MIT License
+ * Copyright (C) Photon Vision.
  *
- * Copyright (c) PhotonVision
+ * 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.
  *
- * 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:
+ * 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.
  *
- * 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.
+ * 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.estimation;
diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
similarity index 81%
rename from photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java
rename to photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
index 3c9c2c152c..387176d57b 100644
--- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java
+++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
@@ -1,25 +1,18 @@
 /*
- * MIT License
+ * Copyright (C) Photon Vision.
  *
- * Copyright (c) PhotonVision
+ * 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.
  *
- * 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:
+ * 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.
  *
- * 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.
+ * 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.estimation;
@@ -36,6 +29,7 @@
 import java.util.Objects;
 import java.util.stream.Collectors;
 import org.opencv.core.Point;
+import org.photonvision.targeting.PNPResults;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
 
diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
new file mode 100644
index 0000000000..453c0a57e5
--- /dev/null
+++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
@@ -0,0 +1,93 @@
+/*
+ * 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.targeting;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.photonvision.common.dataflow.structures.Packet;
+
+public class MultiTargetPNPResults {
+    // Seeing 32 apriltags at once seems like a sane limit
+    private static final int MAX_IDS = 32;
+    // pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
+    public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS);
+
+    public PNPResults estimatedPose = new PNPResults();
+    public List<Integer> fiducialIDsUsed = List.of();
+
+    public MultiTargetPNPResults() {}
+
+    public MultiTargetPNPResults(PNPResults results, List<Integer> ids) {
+        estimatedPose = results;
+        fiducialIDsUsed = ids;
+    }
+
+    public static MultiTargetPNPResults createFromPacket(Packet packet) {
+        var results = PNPResults.createFromPacket(packet);
+        var ids = new ArrayList<Integer>(MAX_IDS);
+        for (int i = 0; i < MAX_IDS; i++) {
+            int targetId = (int) packet.decodeShort();
+            if (targetId > -1) ids.add(targetId);
+        }
+        return new MultiTargetPNPResults(results, ids);
+    }
+
+    public void populatePacket(Packet packet) {
+        estimatedPose.populatePacket(packet);
+        for (int i = 0; i < MAX_IDS; i++) {
+            if (i < fiducialIDsUsed.size()) {
+                packet.encode((short) fiducialIDsUsed.get(i).byteValue());
+            } else {
+                packet.encode((short) -1);
+            }
+        }
+    }
+
+    @Override
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        result = prime * result + ((estimatedPose == null) ? 0 : estimatedPose.hashCode());
+        result = prime * result + ((fiducialIDsUsed == null) ? 0 : fiducialIDsUsed.hashCode());
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj == null) return false;
+        if (getClass() != obj.getClass()) return false;
+        MultiTargetPNPResults other = (MultiTargetPNPResults) obj;
+        if (estimatedPose == null) {
+            if (other.estimatedPose != null) return false;
+        } else if (!estimatedPose.equals(other.estimatedPose)) return false;
+        if (fiducialIDsUsed == null) {
+            if (other.fiducialIDsUsed != null) return false;
+        } else if (!fiducialIDsUsed.equals(other.fiducialIDsUsed)) return false;
+        return true;
+    }
+
+    @Override
+    public String toString() {
+        return "MultiTargetPNPResults [estimatedPose="
+                + estimatedPose
+                + ", fiducialIDsUsed="
+                + fiducialIDsUsed
+                + "]";
+    }
+}
diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java
new file mode 100644
index 0000000000..11a77547fc
--- /dev/null
+++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java
@@ -0,0 +1,170 @@
+/*
+ * 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.targeting;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import org.photonvision.common.dataflow.structures.Packet;
+import org.photonvision.utils.PacketUtils;
+
+/**
+ * The best estimated transformation from solvePnP, and possibly an alternate transformation
+ * depending on the solvePNP method. If an alternate solution is present, the ambiguity value
+ * represents the ratio of reprojection error in the best solution to the alternate (best /
+ * alternate).
+ *
+ * <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
+ * method.
+ */
+public class PNPResults {
+    /**
+     * If this result is valid. A false value indicates there was an error in estimation, and this
+     * result should not be used.
+     */
+    public final boolean isPresent;
+
+    /**
+     * The best-fit transform. The coordinate frame of this transform depends on the method which gave
+     * this result.
+     */
+    public final Transform3d best;
+
+    /** Reprojection error of the best solution, in pixels */
+    public final double bestReprojErr;
+
+    /**
+     * Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
+     * to the best solution.
+     */
+    public final Transform3d alt;
+
+    /** If no alternate solution is found, this is bestReprojErr */
+    public final double altReprojErr;
+
+    /** If no alternate solution is found, this is 0 */
+    public final double ambiguity;
+
+    /** An empty (invalid) result. */
+    public PNPResults() {
+        this.isPresent = false;
+        this.best = new Transform3d();
+        this.alt = new Transform3d();
+        this.ambiguity = 0;
+        this.bestReprojErr = 0;
+        this.altReprojErr = 0;
+    }
+
+    public PNPResults(Transform3d best, double bestReprojErr) {
+        this(best, best, 0, bestReprojErr, bestReprojErr);
+    }
+
+    public PNPResults(
+            Transform3d best,
+            Transform3d alt,
+            double ambiguity,
+            double bestReprojErr,
+            double altReprojErr) {
+        this.isPresent = true;
+        this.best = best;
+        this.alt = alt;
+        this.ambiguity = ambiguity;
+        this.bestReprojErr = bestReprojErr;
+        this.altReprojErr = altReprojErr;
+    }
+
+    public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3);
+
+    public static PNPResults createFromPacket(Packet packet) {
+        var present = packet.decodeBoolean();
+        var best = PacketUtils.decodeTransform(packet);
+        var alt = PacketUtils.decodeTransform(packet);
+        var bestEr = packet.decodeDouble();
+        var altEr = packet.decodeDouble();
+        var ambiguity = packet.decodeDouble();
+        if (present) {
+            return new PNPResults(best, alt, ambiguity, bestEr, altEr);
+        } else {
+            return new PNPResults();
+        }
+    }
+
+    public Packet populatePacket(Packet packet) {
+        packet.encode(isPresent);
+        PacketUtils.encodeTransform(packet, best);
+        PacketUtils.encodeTransform(packet, alt);
+        packet.encode(bestReprojErr);
+        packet.encode(altReprojErr);
+        packet.encode(ambiguity);
+        return packet;
+    }
+
+    @Override
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        result = prime * result + (isPresent ? 1231 : 1237);
+        result = prime * result + ((best == null) ? 0 : best.hashCode());
+        long temp;
+        temp = Double.doubleToLongBits(bestReprojErr);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        result = prime * result + ((alt == null) ? 0 : alt.hashCode());
+        temp = Double.doubleToLongBits(altReprojErr);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(ambiguity);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj == null) return false;
+        if (getClass() != obj.getClass()) return false;
+        PNPResults other = (PNPResults) obj;
+        if (isPresent != other.isPresent) return false;
+        if (best == null) {
+            if (other.best != null) return false;
+        } else if (!best.equals(other.best)) return false;
+        if (Double.doubleToLongBits(bestReprojErr) != Double.doubleToLongBits(other.bestReprojErr))
+            return false;
+        if (alt == null) {
+            if (other.alt != null) return false;
+        } else if (!alt.equals(other.alt)) return false;
+        if (Double.doubleToLongBits(altReprojErr) != Double.doubleToLongBits(other.altReprojErr))
+            return false;
+        if (Double.doubleToLongBits(ambiguity) != Double.doubleToLongBits(other.ambiguity))
+            return false;
+        return true;
+    }
+
+    @Override
+    public String toString() {
+        return "PNPResults [isPresent="
+                + isPresent
+                + ", best="
+                + best
+                + ", bestReprojErr="
+                + bestReprojErr
+                + ", alt="
+                + alt
+                + ", altReprojErr="
+                + altReprojErr
+                + ", ambiguity="
+                + ambiguity
+                + "]";
+    }
+}
diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
index f2a5292454..4472078f8a 100644
--- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
+++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
@@ -34,6 +34,9 @@ public class PhotonPipelineResult {
     // Timestamp in milliseconds.
     private double timestampSeconds = -1;
 
+    // Multi-tag result
+    private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
+
     /** Constructs an empty pipeline result. */
     public PhotonPipelineResult() {}
 
@@ -48,13 +51,30 @@ public PhotonPipelineResult(double latencyMillis, List<PhotonTrackedTarget> targ
         this.targets.addAll(targets);
     }
 
+    /**
+     * Constructs a pipeline result.
+     *
+     * @param latencyMillis The latency in the pipeline.
+     * @param targets The list of targets identified by the pipeline.
+     * @param result Result from multi-target PNP.
+     */
+    public PhotonPipelineResult(
+            double latencyMillis, List<PhotonTrackedTarget> targets, MultiTargetPNPResults result) {
+        this.latencyMillis = latencyMillis;
+        this.targets.addAll(targets);
+        this.multiTagResult = result;
+    }
+
     /**
      * Returns the size of the packet needed to store this pipeline result.
      *
      * @return The size of the packet needed to store this pipeline result.
      */
     public int getPacketSize() {
-        return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES + 8 + 2;
+        return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES
+                + 8 // latency
+                + MultiTargetPNPResults.PACK_SIZE_BYTES
+                + 1; // target count
     }
 
     /**
@@ -122,6 +142,14 @@ public List<PhotonTrackedTarget> getTargets() {
         return new ArrayList<>(targets);
     }
 
+    /**
+     * Return the latest mulit-target result. Be sure to check
+     * getMultiTagResult().estimatedPose.isPresent before using the pose estimate!
+     */
+    public MultiTargetPNPResults getMultiTagResult() {
+        return multiTagResult;
+    }
+
     /**
      * Populates the fields of the pipeline result from the packet.
      *
@@ -131,6 +159,7 @@ public List<PhotonTrackedTarget> getTargets() {
     public Packet createFromPacket(Packet packet) {
         // Decode latency, existence of targets, and number of targets.
         latencyMillis = packet.decodeDouble();
+        this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet);
         byte targetCount = packet.decodeByte();
 
         targets.clear();
@@ -154,6 +183,7 @@ public Packet createFromPacket(Packet packet) {
     public Packet populatePacket(Packet packet) {
         // Encode latency, existence of targets, and number of targets.
         packet.encode(latencyMillis);
+        multiTagResult.populatePacket(packet);
         packet.encode((byte) targets.size());
 
         // Encode the information of each target.
@@ -173,6 +203,7 @@ public int hashCode() {
         result = prime * result + (int) (temp ^ (temp >>> 32));
         temp = Double.doubleToLongBits(timestampSeconds);
         result = prime * result + (int) (temp ^ (temp >>> 32));
+        result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode());
         return result;
     }
 
@@ -189,6 +220,22 @@ public boolean equals(Object obj) {
             return false;
         if (Double.doubleToLongBits(timestampSeconds)
                 != Double.doubleToLongBits(other.timestampSeconds)) return false;
+        if (multiTagResult == null) {
+            if (other.multiTagResult != null) return false;
+        } else if (!multiTagResult.equals(other.multiTagResult)) return false;
         return true;
     }
+
+    @Override
+    public String toString() {
+        return "PhotonPipelineResult [targets="
+                + targets
+                + ", latencyMillis="
+                + latencyMillis
+                + ", timestampSeconds="
+                + timestampSeconds
+                + ", multiTagResult="
+                + multiTagResult
+                + "]";
+    }
 }
diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
index a15fca87d5..40263f5cc5 100644
--- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
+++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
@@ -17,13 +17,11 @@
 
 package org.photonvision.targeting;
 
-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.geometry.Translation3d;
 import java.util.ArrayList;
 import java.util.List;
 import org.photonvision.common.dataflow.structures.Packet;
+import org.photonvision.utils.PacketUtils;
 
 public class PhotonTrackedTarget {
     private static final int MAX_CORNERS = 8;
@@ -198,29 +196,6 @@ public boolean equals(Object obj) {
         return true;
     }
 
-    private static Transform3d decodeTransform(Packet packet) {
-        double x = packet.decodeDouble();
-        double y = packet.decodeDouble();
-        double z = packet.decodeDouble();
-        var translation = new Translation3d(x, y, z);
-        double w = packet.decodeDouble();
-        x = packet.decodeDouble();
-        y = packet.decodeDouble();
-        z = packet.decodeDouble();
-        var rotation = new Rotation3d(new Quaternion(w, x, y, z));
-        return new Transform3d(translation, rotation);
-    }
-
-    private static void encodeTransform(Packet packet, Transform3d transform) {
-        packet.encode(transform.getTranslation().getX());
-        packet.encode(transform.getTranslation().getY());
-        packet.encode(transform.getTranslation().getZ());
-        packet.encode(transform.getRotation().getQuaternion().getW());
-        packet.encode(transform.getRotation().getQuaternion().getX());
-        packet.encode(transform.getRotation().getQuaternion().getY());
-        packet.encode(transform.getRotation().getQuaternion().getZ());
-    }
-
     private static void encodeList(Packet packet, List<TargetCorner> list) {
         packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE));
         for (int i = 0; i < list.size(); i++) {
@@ -253,8 +228,8 @@ public Packet createFromPacket(Packet packet) {
         this.skew = packet.decodeDouble();
         this.fiducialId = packet.decodeInt();
 
-        this.bestCameraToTarget = decodeTransform(packet);
-        this.altCameraToTarget = decodeTransform(packet);
+        this.bestCameraToTarget = PacketUtils.decodeTransform(packet);
+        this.altCameraToTarget = PacketUtils.decodeTransform(packet);
 
         this.poseAmbiguity = packet.decodeDouble();
 
@@ -282,8 +257,8 @@ public Packet populatePacket(Packet packet) {
         packet.encode(area);
         packet.encode(skew);
         packet.encode(fiducialId);
-        encodeTransform(packet, bestCameraToTarget);
-        encodeTransform(packet, altCameraToTarget);
+        PacketUtils.encodeTransform(packet, bestCameraToTarget);
+        PacketUtils.encodeTransform(packet, altCameraToTarget);
         packet.encode(poseAmbiguity);
 
         for (int i = 0; i < 4; i++) {
diff --git a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java
new file mode 100644
index 0000000000..43a0dd4cf6
--- /dev/null
+++ b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java
@@ -0,0 +1,49 @@
+/*
+ * 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.utils;
+
+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.geometry.Translation3d;
+import org.photonvision.common.dataflow.structures.Packet;
+
+public class PacketUtils {
+    public static Transform3d decodeTransform(Packet packet) {
+        double x = packet.decodeDouble();
+        double y = packet.decodeDouble();
+        double z = packet.decodeDouble();
+        var translation = new Translation3d(x, y, z);
+        double w = packet.decodeDouble();
+        x = packet.decodeDouble();
+        y = packet.decodeDouble();
+        z = packet.decodeDouble();
+        var rotation = new Rotation3d(new Quaternion(w, x, y, z));
+        return new Transform3d(translation, rotation);
+    }
+
+    public static void encodeTransform(Packet packet, Transform3d transform) {
+        packet.encode(transform.getTranslation().getX());
+        packet.encode(transform.getTranslation().getY());
+        packet.encode(transform.getTranslation().getZ());
+        packet.encode(transform.getRotation().getQuaternion().getW());
+        packet.encode(transform.getRotation().getQuaternion().getX());
+        packet.encode(transform.getRotation().getQuaternion().getY());
+        packet.encode(transform.getRotation().getQuaternion().getZ());
+    }
+}
diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
index 5caab2ad53..c96ff8078d 100644
--- a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
+++ b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
@@ -36,8 +36,8 @@ class PhotonCameraWrapper {
  public:
   photonlib::PhotonPoseEstimator m_poseEstimator{
       frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
-      photonlib::MULTI_TAG_PNP, std::move(photonlib::PhotonCamera{"WPI2023"}),
-      frc::Transform3d{}};
+      photonlib::MULTI_TAG_PNP_ON_RIO,
+      std::move(photonlib::PhotonCamera{"WPI2023"}), frc::Transform3d{}};
 
   inline std::optional<photonlib::EstimatedRobotPose> Update(
       frc::Pose2d estimatedPose) {
diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java
index 1d9b2ab9ed..d1e88c9258 100644
--- a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java
+++ b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java
@@ -56,7 +56,8 @@ public Vision() {
         camera = new PhotonCamera(kCameraName);
 
         photonEstimator =
-                new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP, camera, kRobotToCam);
+                new PhotonPoseEstimator(
+                        kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, kRobotToCam);
         photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
 
         // ----- Simulation