Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Stdev-UI: Handle angle wrapping #1061

Merged
merged 2 commits into from
Dec 28, 2023
Merged

Stdev-UI: Handle angle wrapping #1061

Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
28fba4b
Handle angle wrapping
mcm001 Dec 20, 2023
8f1f9c7
Merge branch 'master' into 2023-11-21_stdev-in-ui
srimanachanta Dec 28, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 33 additions & 13 deletions photon-client/src/components/dashboard/tabs/TargetsTab.vue
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,12 +3,32 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
import { PipelineType } from "@/types/PipelineTypes";
import { useStateStore } from "@/stores/StateStore";

const wrapToPi = (delta: number): number => {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I feel like this should be renamed to angleModulus to match the method in MathUtils, I feel like that name is more indicative of what the method does than wrapToPi.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think both are fine, but I actually think this is more descriptive? IDK

let ret = delta;
while (ret < -Math.PI) ret += Math.PI * 2;
while (ret > Math.PI) ret -= Math.PI * 2;
return ret;
};

const calculateStdDev = (values: number[]): number => {
if (values.length < 2) return 0;

const mean = values.reduce((sum, number) => sum + number, 0) / values.length;
// Use mean of cosine/sine components to handle angle wrapping
const cosines = values.map((it) => Math.cos(it));
const sines = values.map((it) => Math.sin(it));
const cosmean = cosines.reduce((sum, number) => sum + number, 0) / values.length;
const sinmean = sines.reduce((sum, number) => sum + number, 0) / values.length;

// Borrowed from WPILib's Rotation2d
const hypot = Math.hypot(cosmean, sinmean);
let mean;
if (hypot > 1e-6) {
mean = Math.atan2(sinmean / hypot, cosmean / hypot);
} else {
mean = 0;
}

return Math.sqrt(values.map((x) => Math.pow(x - mean, 2)).reduce((a, b) => a + b) / values.length);
return Math.sqrt(values.map((x) => Math.pow(wrapToPi(x - mean), 2)).reduce((a, b) => a + b) / values.length);
};
const resetCurrentBuffer = () => {
// Need to clear the array in place
Expand Down Expand Up @@ -119,23 +139,23 @@ const resetCurrentBuffer = () => {
<td>
{{
(
useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x *
(useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x || 0) *
(180.0 / Math.PI)
).toFixed(2)
}}&deg;
</td>
<td>
{{
(
useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y *
(useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y || 0) *
(180.0 / Math.PI)
).toFixed(2)
}}&deg;
</td>
<td>
{{
(
useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z *
(useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z || 0) *
(180.0 / Math.PI)
).toFixed(2)
}}&deg;
Expand All @@ -146,8 +166,8 @@ const resetCurrentBuffer = () => {
</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
>Multi-tag pose standard deviation over the last
{{ useStateStore().currentMultitagBuffer?.length || "NaN" }}/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
Expand All @@ -164,37 +184,37 @@ const resetCurrentBuffer = () => {
<tbody v-show="useStateStore().currentPipelineResults?.multitagResult">
<td>
{{
calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x)).toFixed(5)
calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x) || []).toFixed(5)
}}&nbsp;m
</td>
<td>
{{
calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y)).toFixed(5)
calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y) || []).toFixed(5)
}}&nbsp;m
</td>
<td>
{{
calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z)).toFixed(5)
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))
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))
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))
useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI)) || []
).toFixed(5)
}}&deg;
</td>
Expand Down