Skip to content

Commit 954ca9a

Browse files
mcm001srimanachanta
mcm001
and
srimanachanta
authored
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]>
1 parent 796b8e7 commit 954ca9a

File tree

4 files changed

+142
-30
lines changed

4 files changed

+142
-30
lines changed

‎photon-client/src/components/dashboard/tabs/TargetsTab.vue

Lines changed: 117 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -3,13 +3,22 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
33
import { PipelineType } from "@/types/PipelineTypes";
44
import { useStateStore } from "@/stores/StateStore";
55
6-
const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings;
6+
const calculateStdDev = (values: number[]): number => {
7+
if (values.length < 2) return 0;
8+
9+
const mean = values.reduce((sum, number) => sum + number, 0) / values.length;
10+
11+
return Math.sqrt(values.map((x) => Math.pow(x - mean, 2)).reduce((a, b) => a + b) / values.length);
12+
};
13+
const resetCurrentBuffer = () => {
14+
// Need to clear the array in place
15+
while (useStateStore().currentMultitagBuffer?.length != 0) useStateStore().currentMultitagBuffer?.pop();
16+
};
717
</script>
818

919
<template>
1020
<div>
1121
<v-row align="start" class="pb-4" style="height: 300px">
12-
<!-- Simple table height must be set here and in the CSS for the fixed-header to work -->
1322
<v-simple-table fixed-header dense dark>
1423
<template #default>
1524
<thead style="font-size: 1.25rem">
@@ -80,40 +89,119 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings
8089
</template>
8190
</v-simple-table>
8291
</v-row>
83-
<v-row
92+
<v-container
8493
v-if="
8594
(useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag ||
8695
useCameraSettingsStore().currentPipelineType === PipelineType.Aruco) &&
87-
currentPipelineSettings.doMultiTarget &&
96+
useCameraSettingsStore().currentPipelineSettings.doMultiTarget &&
8897
useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
8998
useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled
9099
"
91-
align="start"
92-
class="pb-4 white--text"
93100
>
94-
<v-card-subtitle class="ma-0 pa-0 pb-4" style="font-size: 16px">Multi-tag pose, field-to-camera</v-card-subtitle>
95-
<v-simple-table fixed-header height="100%" dense dark>
96-
<thead style="font-size: 1.25rem">
97-
<th class="text-center">X meters</th>
98-
<th class="text-center">Y meters</th>
99-
<th class="text-center">Z Angle &theta;&deg;</th>
100-
<th class="text-center">Tags</th>
101-
</thead>
102-
<tbody v-show="useStateStore().currentPipelineResults?.multitagResult">
103-
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }}&nbsp;m</td>
104-
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }}&nbsp;m</td>
105-
<td>
106-
{{
107-
(
108-
useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z *
109-
(180.0 / Math.PI)
110-
).toFixed(2)
111-
}}&deg;
112-
</td>
113-
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.fiducialIDsUsed }}</td>
114-
</tbody>
115-
</v-simple-table>
116-
</v-row>
101+
<v-row class="pb-4 white--text">
102+
<v-card-subtitle class="ma-0 pa-0 pb-4" style="font-size: 16px"
103+
>Multi-tag pose, field-to-camera</v-card-subtitle
104+
>
105+
<v-simple-table fixed-header height="100%" dense dark>
106+
<thead style="font-size: 1.25rem">
107+
<th class="text-center">X meters</th>
108+
<th class="text-center">Y meters</th>
109+
<th class="text-center">Z meters</th>
110+
<th class="text-center">X Angle &theta;&deg;</th>
111+
<th class="text-center">Y Angle &theta;&deg;</th>
112+
<th class="text-center">Z Angle &theta;&deg;</th>
113+
<th class="text-center">Tags</th>
114+
</thead>
115+
<tbody v-show="useStateStore().currentPipelineResults?.multitagResult">
116+
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }}&nbsp;m</td>
117+
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }}&nbsp;m</td>
118+
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.z.toFixed(2) }}&nbsp;m</td>
119+
<td>
120+
{{
121+
(
122+
useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x *
123+
(180.0 / Math.PI)
124+
).toFixed(2)
125+
}}&deg;
126+
</td>
127+
<td>
128+
{{
129+
(
130+
useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y *
131+
(180.0 / Math.PI)
132+
).toFixed(2)
133+
}}&deg;
134+
</td>
135+
<td>
136+
{{
137+
(
138+
useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z *
139+
(180.0 / Math.PI)
140+
).toFixed(2)
141+
}}&deg;
142+
</td>
143+
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.fiducialIDsUsed }}</td>
144+
</tbody>
145+
</v-simple-table>
146+
</v-row>
147+
<v-row class="pb-4 white--text" style="display: flex; flex-direction: column">
148+
<v-card-subtitle class="ma-0 pa-0 pb-4 pr-4" style="font-size: 16px"
149+
>Multi-tag pose standard deviation over the last {{ useStateStore().currentMultitagBuffer.length }}/100
150+
samples
151+
</v-card-subtitle>
152+
<v-btn color="secondary" class="mb-4 mt-1" style="width: min-content" depressed @click="resetCurrentBuffer"
153+
>Reset Samples</v-btn
154+
>
155+
<v-simple-table fixed-header height="100%" dense dark>
156+
<thead style="font-size: 1.25rem">
157+
<th class="text-center">X meters</th>
158+
<th class="text-center">Y meters</th>
159+
<th class="text-center">Z meters</th>
160+
<th class="text-center">X Angle &theta;&deg;</th>
161+
<th class="text-center">Y Angle &theta;&deg;</th>
162+
<th class="text-center">Z Angle &theta;&deg;</th>
163+
</thead>
164+
<tbody v-show="useStateStore().currentPipelineResults?.multitagResult">
165+
<td>
166+
{{
167+
calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x)).toFixed(5)
168+
}}&nbsp;m
169+
</td>
170+
<td>
171+
{{
172+
calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y)).toFixed(5)
173+
}}&nbsp;m
174+
</td>
175+
<td>
176+
{{
177+
calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z)).toFixed(5)
178+
}}&nbsp;m
179+
</td>
180+
<td>
181+
{{
182+
calculateStdDev(
183+
useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_x * (180.0 / Math.PI))
184+
).toFixed(5)
185+
}}&deg;
186+
</td>
187+
<td>
188+
{{
189+
calculateStdDev(
190+
useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_y * (180.0 / Math.PI))
191+
).toFixed(5)
192+
}}&deg;
193+
</td>
194+
<td>
195+
{{
196+
calculateStdDev(
197+
useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI))
198+
).toFixed(5)
199+
}}&deg;
200+
</td>
201+
</tbody>
202+
</v-simple-table>
203+
</v-row>
204+
</v-container>
117205
</div>
118206
</template>
119207

‎photon-client/src/stores/StateStore.ts

Lines changed: 21 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
import { defineStore } from "pinia";
22
import type { LogMessage } from "@/types/SettingTypes";
33
import type { AutoReconnectingWebsocket } from "@/lib/AutoReconnectingWebsocket";
4-
import type { PipelineResult } from "@/types/PhotonTrackingTypes";
4+
import type { MultitagResult, PipelineResult } from "@/types/PhotonTrackingTypes";
55
import type {
66
WebsocketCalibrationData,
77
WebsocketLogMessage,
@@ -25,6 +25,7 @@ interface StateStore {
2525
currentCameraIndex: number;
2626

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

2930
colorPickingMode: boolean;
3031

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

6162
backendResults: {},
63+
multitagResultBuffer: {},
6264

6365
colorPickingMode: false,
6466

@@ -80,6 +82,9 @@ export const useStateStore = defineStore("state", {
8082
getters: {
8183
currentPipelineResults(): PipelineResult | undefined {
8284
return this.backendResults[this.currentCameraIndex.toString()];
85+
},
86+
currentMultitagBuffer(): MultitagResult[] | undefined {
87+
return this.multitagResultBuffer[this.currentCameraIndex.toString()];
8388
}
8489
},
8590
actions: {
@@ -105,6 +110,21 @@ export const useStateStore = defineStore("state", {
105110
...this.backendResults,
106111
...data
107112
};
113+
114+
for (const key in data) {
115+
const multitagRes = data[key].multitagResult;
116+
117+
if (multitagRes) {
118+
if (!this.multitagResultBuffer[key]) {
119+
this.multitagResultBuffer[key] = [];
120+
}
121+
122+
this.multitagResultBuffer[key].push(multitagRes);
123+
if (this.multitagResultBuffer[key].length > 100) {
124+
this.multitagResultBuffer[key].shift();
125+
}
126+
}
127+
}
108128
},
109129
updateCalibrationStateValuesFromWebsocket(data: WebsocketCalibrationData) {
110130
this.calibrationData = {

‎photon-client/src/types/PhotonTrackingTypes.ts

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,8 @@ export interface Transform3d {
66
qx: number;
77
qy: number;
88
qz: number;
9+
angle_x: number;
10+
angle_y: number;
911
angle_z: number;
1012
}
1113

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,8 @@ public static HashMap<String, Object> transformToHashMap(Transform3d transform)
5555
ret.put("qy", transform.getRotation().getQuaternion().getY());
5656
ret.put("qz", transform.getRotation().getQuaternion().getZ());
5757

58+
ret.put("angle_x", transform.getRotation().getX());
59+
ret.put("angle_y", transform.getRotation().getY());
5860
ret.put("angle_z", transform.getRotation().getZ());
5961
return ret;
6062
}

0 commit comments

Comments
 (0)