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 <[email protected]>
PhotonVision · Dec 19, 2023 · 954ca9a · 954ca9a
1 parent 796b8e7
commit 954ca9a
Show file tree

Hide file tree

Showing 4 changed files with 142 additions and 30 deletions.
diff --git 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>
 

diff --git 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 = {

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

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
@@ -55,6 +55,8 @@ public static HashMap<String, Object> transformToHashMap(Transform3d transform)
         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;
     }