Record standard deviations for multi-tag pose (#1019)

* Record standard deviations for multi-tag pose * Add XYZ translation and angle to stdev uI * create multitag result buffer in store allows results to be stored in the background * simplify logic in targeting tab also adds a reset button * Formatting fixes * convert rad angles to deg --------- Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com>
2026-07-03 03:01:40 +00:00 · 2023-12-19 15:47:53 -05:00
parent 796b8e73d5
commit 954ca9a577
4 changed files with 142 additions and 30 deletions
--- a/photon-client/src/components/dashboard/tabs/TargetsTab.vue
+++ b/photon-client/src/components/dashboard/tabs/TargetsTab.vue
@@ -3,13 +3,22 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { PipelineType } from "@/types/PipelineTypes";
 import { useStateStore } from "@/stores/StateStore";

-const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings;
+const calculateStdDev = (values: number[]): number => {
+  if (values.length < 2) return 0;
+
+  const mean = values.reduce((sum, number) => sum + number, 0) / values.length;
+
+  return Math.sqrt(values.map((x) => Math.pow(x - mean, 2)).reduce((a, b) => a + b) / values.length);
+};
+const resetCurrentBuffer = () => {
+  // Need to clear the array in place
+  while (useStateStore().currentMultitagBuffer?.length != 0) useStateStore().currentMultitagBuffer?.pop();
+};
 </script>

 <template>
  <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 dense dark>
        <template #default>
          <thead style="font-size: 1.25rem">
@@ -80,40 +89,119 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings
        </template>
      </v-simple-table>
    </v-row>
-    <v-row
+    <v-container
      v-if="
        (useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag ||
          useCameraSettingsStore().currentPipelineType === PipelineType.Aruco) &&
-        currentPipelineSettings.doMultiTarget &&
+        useCameraSettingsStore().currentPipelineSettings.doMultiTarget &&
        useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
        useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled
      "
-      align="start"
-      class="pb-4 white--text"
    >
-      <v-card-subtitle class="ma-0 pa-0 pb-4" style="font-size: 16px">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 v-show="useStateStore().currentPipelineResults?.multitagResult">
-          <td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }}&nbsp;m</td>
-          <td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }}&nbsp;m</td>
-          <td>
-            {{
-              (
-                useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z *
-                (180.0 / Math.PI)
-              ).toFixed(2)
-            }}&deg;
-          </td>
-          <td>{{ useStateStore().currentPipelineResults?.multitagResult?.fiducialIDsUsed }}</td>
-        </tbody>
-      </v-simple-table>
-    </v-row>
+      <v-row class="pb-4 white--text">
+        <v-card-subtitle class="ma-0 pa-0 pb-4" style="font-size: 16px"
+          >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 meters</th>
+            <th class="text-center">X Angle &theta;&deg;</th>
+            <th class="text-center">Y Angle &theta;&deg;</th>
+            <th class="text-center">Z Angle &theta;&deg;</th>
+            <th class="text-center">Tags</th>
+          </thead>
+          <tbody v-show="useStateStore().currentPipelineResults?.multitagResult">
+            <td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }}&nbsp;m</td>
+            <td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }}&nbsp;m</td>
+            <td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.z.toFixed(2) }}&nbsp;m</td>
+            <td>
+              {{
+                (
+                  useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x *
+                  (180.0 / Math.PI)
+                ).toFixed(2)
+              }}&deg;
+            </td>
+            <td>
+              {{
+                (
+                  useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y *
+                  (180.0 / Math.PI)
+                ).toFixed(2)
+              }}&deg;
+            </td>
+            <td>
+              {{
+                (
+                  useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z *
+                  (180.0 / Math.PI)
+                ).toFixed(2)
+              }}&deg;
+            </td>
+            <td>{{ useStateStore().currentPipelineResults?.multitagResult?.fiducialIDsUsed }}</td>
+          </tbody>
+        </v-simple-table>
+      </v-row>
+      <v-row class="pb-4 white--text" style="display: flex; flex-direction: column">
+        <v-card-subtitle class="ma-0 pa-0 pb-4 pr-4" style="font-size: 16px"
+          >Multi-tag pose standard deviation over the last {{ useStateStore().currentMultitagBuffer.length }}/100
+          samples
+        </v-card-subtitle>
+        <v-btn color="secondary" class="mb-4 mt-1" style="width: min-content" depressed @click="resetCurrentBuffer"
+          >Reset Samples</v-btn
+        >
+        <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 meters</th>
+            <th class="text-center">X Angle &theta;&deg;</th>
+            <th class="text-center">Y Angle &theta;&deg;</th>
+            <th class="text-center">Z Angle &theta;&deg;</th>
+          </thead>
+          <tbody v-show="useStateStore().currentPipelineResults?.multitagResult">
+            <td>
+              {{
+                calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x)).toFixed(5)
+              }}&nbsp;m
+            </td>
+            <td>
+              {{
+                calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y)).toFixed(5)
+              }}&nbsp;m
+            </td>
+            <td>
+              {{
+                calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z)).toFixed(5)
+              }}&nbsp;m
+            </td>
+            <td>
+              {{
+                calculateStdDev(
+                  useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_x * (180.0 / Math.PI))
+                ).toFixed(5)
+              }}&deg;
+            </td>
+            <td>
+              {{
+                calculateStdDev(
+                  useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_y * (180.0 / Math.PI))
+                ).toFixed(5)
+              }}&deg;
+            </td>
+            <td>
+              {{
+                calculateStdDev(
+                  useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI))
+                ).toFixed(5)
+              }}&deg;
+            </td>
+          </tbody>
+        </v-simple-table>
+      </v-row>
+    </v-container>
  </div>
 </template>

--- a/photon-client/src/stores/StateStore.ts
+++ b/photon-client/src/stores/StateStore.ts
@@ -1,7 +1,7 @@
 import { defineStore } from "pinia";
 import type { LogMessage } from "@/types/SettingTypes";
 import type { AutoReconnectingWebsocket } from "@/lib/AutoReconnectingWebsocket";
-import type { PipelineResult } from "@/types/PhotonTrackingTypes";
+import type { MultitagResult, PipelineResult } from "@/types/PhotonTrackingTypes";
 import type {
  WebsocketCalibrationData,
  WebsocketLogMessage,
@@ -25,6 +25,7 @@ interface StateStore {
  currentCameraIndex: number;

  backendResults: Record<string, PipelineResult>;
+  multitagResultBuffer: Record<string, MultitagResult[]>;

  colorPickingMode: boolean;

@@ -59,6 +60,7 @@ export const useStateStore = defineStore("state", {
      currentCameraIndex: 0,

      backendResults: {},
+      multitagResultBuffer: {},

      colorPickingMode: false,

@@ -80,6 +82,9 @@ export const useStateStore = defineStore("state", {
  getters: {
    currentPipelineResults(): PipelineResult | undefined {
      return this.backendResults[this.currentCameraIndex.toString()];
+    },
+    currentMultitagBuffer(): MultitagResult[] | undefined {
+      return this.multitagResultBuffer[this.currentCameraIndex.toString()];
    }
  },
  actions: {
@@ -105,6 +110,21 @@ export const useStateStore = defineStore("state", {
        ...this.backendResults,
        ...data
      };
+
+      for (const key in data) {
+        const multitagRes = data[key].multitagResult;
+
+        if (multitagRes) {
+          if (!this.multitagResultBuffer[key]) {
+            this.multitagResultBuffer[key] = [];
+          }
+
+          this.multitagResultBuffer[key].push(multitagRes);
+          if (this.multitagResultBuffer[key].length > 100) {
+            this.multitagResultBuffer[key].shift();
+          }
+        }
+      }
    },
    updateCalibrationStateValuesFromWebsocket(data: WebsocketCalibrationData) {
      this.calibrationData = {
--- a/photon-client/src/types/PhotonTrackingTypes.ts
+++ b/photon-client/src/types/PhotonTrackingTypes.ts
@@ -6,6 +6,8 @@ export interface Transform3d {
  qx: number;
  qy: number;
  qz: number;
+  angle_x: number;
+  angle_y: number;
  angle_z: number;
 }

--- a/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java
+++ b/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java
@@ -55,6 +55,8 @@ public final class SerializationUtils {
        ret.put("qy", transform.getRotation().getQuaternion().getY());
        ret.put("qz", transform.getRotation().getQuaternion().getZ());

+        ret.put("angle_x", transform.getRotation().getX());
+        ret.put("angle_y", transform.getRotation().getY());
        ret.put("angle_z", transform.getRotation().getZ());
        return ret;
    }