mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-30 02:31:40 +00:00
Aruco Support for AprilTag Detection (Experimental) (#636)
Uses OpenCV's aruco module for AprilTag detection.
This commit is contained in:
Mohammad Durrani
@@ -153,7 +153,7 @@
|
||||
v-model="currentPipelineType"
|
||||
name="Type"
|
||||
tooltip="Changes the pipeline type, which changes the type of processing that will happen on input frames"
|
||||
:list="['Reflective Tape', 'Colored Shape', 'AprilTag']"
|
||||
:list="['Reflective Tape', 'Colored Shape', 'AprilTag', 'Aruco']"
|
||||
@input="e => showTypeDialog(e)"
|
||||
/>
|
||||
</v-col>
|
||||
|
||||
@@ -52,7 +52,7 @@ export default new Vuex.Store({
|
||||
isFovConfigurable: true,
|
||||
calibrated: false,
|
||||
currentPipelineSettings: {
|
||||
pipelineType: 4, // One of "calib", "driver", "reflective", "shape", "AprilTag"
|
||||
pipelineType: 5, // One of "calib", "driver", "reflective", "shape", "AprilTag"
|
||||
// 2 is reflective
|
||||
|
||||
// Settings that apply to all pipeline types
|
||||
@@ -91,7 +91,7 @@ export default new Vuex.Store({
|
||||
cornerDetectionAccuracyPercentage: 10,
|
||||
|
||||
// Settings that apply to AprilTag
|
||||
tagFamily: 0,
|
||||
tagFamily: 1,
|
||||
decimate: 1.0,
|
||||
blur: 0.0,
|
||||
threads: 1,
|
||||
|
||||
@@ -1,37 +1,37 @@
|
||||
<template>
|
||||
<div>
|
||||
<v-container
|
||||
class="pa-3"
|
||||
fluid
|
||||
class="pa-3"
|
||||
fluid
|
||||
>
|
||||
<v-row
|
||||
no-gutters
|
||||
align="center"
|
||||
justify="center"
|
||||
no-gutters
|
||||
align="center"
|
||||
justify="center"
|
||||
>
|
||||
<v-col
|
||||
cols="12"
|
||||
:class="['pb-3 ', 'pr-lg-3']"
|
||||
lg="8"
|
||||
align-self="stretch"
|
||||
cols="12"
|
||||
:class="['pb-3 ', 'pr-lg-3']"
|
||||
lg="8"
|
||||
align-self="stretch"
|
||||
>
|
||||
<v-card
|
||||
color="primary"
|
||||
height="100%"
|
||||
style="display: flex; flex-direction: column"
|
||||
dark
|
||||
color="primary"
|
||||
height="100%"
|
||||
style="display: flex; flex-direction: column"
|
||||
dark
|
||||
>
|
||||
<v-card-title
|
||||
class="pb-0 mb-0 pl-4 pt-1"
|
||||
style="height: 15%; min-height: 50px;"
|
||||
class="pb-0 mb-0 pl-4 pt-1"
|
||||
style="height: 15%; min-height: 50px;"
|
||||
>
|
||||
Cameras
|
||||
<v-chip
|
||||
:class="fpsTooLow ? 'ml-2 mt-1' : 'mt-2'"
|
||||
x-small
|
||||
label
|
||||
:color="fpsTooLow ? 'error' : 'transparent'"
|
||||
:text-color="fpsTooLow ? 'white' : 'grey'"
|
||||
:class="fpsTooLow ? 'ml-2 mt-1' : 'mt-2'"
|
||||
x-small
|
||||
label
|
||||
:color="fpsTooLow ? 'error' : 'transparent'"
|
||||
:text-color="fpsTooLow ? 'white' : 'grey'"
|
||||
>
|
||||
<span class="pr-1">Processing @ {{ Math.round($store.state.pipelineResults.fps) }} FPS –</span>
|
||||
<span v-if="fpsTooLow && !$store.getters.currentPipelineSettings.inputShouldShow && $store.getters.pipelineType == 2">HSV thresholds are too broad; narrow them for better performance</span>
|
||||
@@ -39,37 +39,37 @@
|
||||
<span v-else>{{ Math.min(Math.round($store.state.pipelineResults.latency), 9999) }} ms latency</span>
|
||||
</v-chip>
|
||||
<v-switch
|
||||
v-model="driverMode"
|
||||
label="Driver Mode"
|
||||
style="margin-left: auto;"
|
||||
color="accent"
|
||||
v-model="driverMode"
|
||||
label="Driver Mode"
|
||||
style="margin-left: auto;"
|
||||
color="accent"
|
||||
/>
|
||||
</v-card-title>
|
||||
<v-row
|
||||
align="center"
|
||||
align="center"
|
||||
>
|
||||
<v-col
|
||||
v-for="idx in (selectedOutputs instanceof Array ? selectedOutputs : [selectedOutputs])"
|
||||
:key="idx"
|
||||
cols="12"
|
||||
:md="selectedOutputs.length === 1 ? 12 : Math.floor(12 / selectedOutputs.length)"
|
||||
class="pb-0 pt-0"
|
||||
style="height: 100%;"
|
||||
v-for="idx in (selectedOutputs instanceof Array ? selectedOutputs : [selectedOutputs])"
|
||||
:key="idx"
|
||||
cols="12"
|
||||
:md="selectedOutputs.length === 1 ? 12 : Math.floor(12 / selectedOutputs.length)"
|
||||
class="pb-0 pt-0"
|
||||
style="height: 100%;"
|
||||
>
|
||||
<div style="position: relative; width: 100%; height: 100%;">
|
||||
<cv-image
|
||||
:id="idx === 0 ? 'raw-stream' : 'processed-stream'"
|
||||
ref="streams"
|
||||
:idx=idx
|
||||
:disconnected="!$store.state.backendConnected"
|
||||
scale="100"
|
||||
:max-height="$store.getters.isDriverMode ? '40vh' : '300px'"
|
||||
:max-height-md="$store.getters.isDriverMode ? '50vh' : '380px'"
|
||||
:max-height-lg="$store.getters.isDriverMode ? '55vh' : '390px'"
|
||||
:max-height-xl="$store.getters.isDriverMode ? '60vh' : '450px'"
|
||||
:alt="'Stream ' + idx"
|
||||
:color-picking="$store.state.colorPicking && idx === 0"
|
||||
@click="onImageClick"
|
||||
:id="idx === 0 ? 'raw-stream' : 'processed-stream'"
|
||||
ref="streams"
|
||||
:idx=idx
|
||||
:disconnected="!$store.state.backendConnected"
|
||||
scale="100"
|
||||
:max-height="$store.getters.isDriverMode ? '40vh' : '300px'"
|
||||
:max-height-md="$store.getters.isDriverMode ? '50vh' : '380px'"
|
||||
:max-height-lg="$store.getters.isDriverMode ? '55vh' : '390px'"
|
||||
:max-height-xl="$store.getters.isDriverMode ? '60vh' : '450px'"
|
||||
:alt="'Stream ' + idx"
|
||||
:color-picking="$store.state.colorPicking && idx === 0"
|
||||
@click="onImageClick"
|
||||
/>
|
||||
</div>
|
||||
</v-col>
|
||||
@@ -77,44 +77,44 @@
|
||||
</v-card>
|
||||
</v-col>
|
||||
<v-col
|
||||
cols="12"
|
||||
class="pb-3"
|
||||
lg="4"
|
||||
align-self="stretch"
|
||||
cols="12"
|
||||
class="pb-3"
|
||||
lg="4"
|
||||
align-self="stretch"
|
||||
>
|
||||
<v-card
|
||||
color="primary"
|
||||
color="primary"
|
||||
>
|
||||
<camera-and-pipeline-select />
|
||||
</v-card>
|
||||
<v-card
|
||||
:disabled="$store.getters.isDriverMode || $store.state.colorPicking"
|
||||
class="mt-3"
|
||||
color="primary"
|
||||
:disabled="$store.getters.isDriverMode || $store.state.colorPicking"
|
||||
class="mt-3"
|
||||
color="primary"
|
||||
>
|
||||
<v-row
|
||||
align="center"
|
||||
class="pl-3 pr-3"
|
||||
align="center"
|
||||
class="pl-3 pr-3"
|
||||
>
|
||||
<v-col lg="12">
|
||||
<p style="color: white;">
|
||||
Processing mode:
|
||||
</p>
|
||||
<v-btn-toggle
|
||||
v-model="processingMode"
|
||||
mandatory
|
||||
dark
|
||||
class="fill"
|
||||
v-model="processingMode"
|
||||
mandatory
|
||||
dark
|
||||
class="fill"
|
||||
>
|
||||
<v-btn
|
||||
color="secondary"
|
||||
color="secondary"
|
||||
>
|
||||
<v-icon>mdi-crop-square</v-icon>
|
||||
<span>2D</span>
|
||||
</v-btn>
|
||||
<v-btn
|
||||
color="secondary"
|
||||
@click="on3DClick"
|
||||
color="secondary"
|
||||
@click="on3DClick"
|
||||
>
|
||||
<v-icon>mdi-cube-outline</v-icon>
|
||||
<span>3D</span>
|
||||
@@ -126,22 +126,22 @@
|
||||
Stream display:
|
||||
</p>
|
||||
<v-btn-toggle
|
||||
v-model="selectedOutputs"
|
||||
:multiple="$vuetify.breakpoint.mdAndUp"
|
||||
mandatory
|
||||
dark
|
||||
class="fill"
|
||||
v-model="selectedOutputs"
|
||||
:multiple="$vuetify.breakpoint.mdAndUp"
|
||||
mandatory
|
||||
dark
|
||||
class="fill"
|
||||
>
|
||||
<v-btn
|
||||
color="secondary"
|
||||
class="fill"
|
||||
color="secondary"
|
||||
class="fill"
|
||||
>
|
||||
<v-icon>mdi-import</v-icon>
|
||||
<span>Raw</span>
|
||||
</v-btn>
|
||||
<v-btn
|
||||
color="secondary"
|
||||
class="fill"
|
||||
color="secondary"
|
||||
class="fill"
|
||||
>
|
||||
<v-icon>mdi-export</v-icon>
|
||||
<span>Processed</span>
|
||||
@@ -154,29 +154,29 @@
|
||||
</v-row>
|
||||
<v-row no-gutters>
|
||||
<v-col
|
||||
v-for="(tabs, idx) in tabGroups"
|
||||
:key="idx"
|
||||
:cols="Math.floor(12 / tabGroups.length)"
|
||||
:class="idx !== tabGroups.length - 1 ? 'pr-3' : ''"
|
||||
align-self="stretch"
|
||||
v-for="(tabs, idx) in tabGroups"
|
||||
:key="idx"
|
||||
:cols="Math.floor(12 / tabGroups.length)"
|
||||
:class="idx !== tabGroups.length - 1 ? 'pr-3' : ''"
|
||||
align-self="stretch"
|
||||
>
|
||||
<v-card
|
||||
color="primary"
|
||||
height="100%"
|
||||
class="pr-4 pl-4"
|
||||
color="primary"
|
||||
height="100%"
|
||||
class="pr-4 pl-4"
|
||||
>
|
||||
<v-tabs
|
||||
v-if="!$store.getters.isDriverMode"
|
||||
v-model="selectedTabs[idx]"
|
||||
grow
|
||||
background-color="primary"
|
||||
dark
|
||||
height="48"
|
||||
slider-color="accent"
|
||||
v-if="!$store.getters.isDriverMode"
|
||||
v-model="selectedTabs[idx]"
|
||||
grow
|
||||
background-color="primary"
|
||||
dark
|
||||
height="48"
|
||||
slider-color="accent"
|
||||
>
|
||||
<v-tab
|
||||
v-for="(tab, i) in tabs"
|
||||
:key="i"
|
||||
v-for="(tab, i) in tabs"
|
||||
:key="i"
|
||||
>
|
||||
{{ tab.name }}
|
||||
</v-tab>
|
||||
@@ -184,10 +184,10 @@
|
||||
<div class="pl-4 pr-4 pt-2">
|
||||
<keep-alive>
|
||||
<component
|
||||
:is="(tabs[selectedTabs[idx]] || tabs[0]).component"
|
||||
:ref="(tabs[selectedTabs[idx]] || tabs[0]).name"
|
||||
v-model="$store.getters.pipeline"
|
||||
@update="$emit('save')"
|
||||
:is="(tabs[selectedTabs[idx]] || tabs[0]).component"
|
||||
:ref="(tabs[selectedTabs[idx]] || tabs[0]).name"
|
||||
v-model="$store.getters.pipeline"
|
||||
@update="$emit('save')"
|
||||
/>
|
||||
</keep-alive>
|
||||
</div>
|
||||
@@ -197,18 +197,18 @@
|
||||
</v-container>
|
||||
|
||||
<v-snackbar
|
||||
v-model="showNTWarning"
|
||||
color="error"
|
||||
timeout="-1"
|
||||
top
|
||||
v-model="showNTWarning"
|
||||
color="error"
|
||||
timeout="-1"
|
||||
top
|
||||
>
|
||||
{{ $store.state.settings.networkSettings.runNTServer ?
|
||||
"NetworkTables server enabled! PhotonLib may not work." :
|
||||
"NetworkTables not connected! Are you on a network with a robot?" }}
|
||||
<template v-slot:action>
|
||||
<v-btn
|
||||
text
|
||||
@click="hideNTWarning = true"
|
||||
text
|
||||
@click="hideNTWarning = true"
|
||||
>
|
||||
Hide
|
||||
</v-btn>
|
||||
@@ -216,12 +216,12 @@
|
||||
</v-snackbar>
|
||||
|
||||
<v-dialog
|
||||
v-model="dialog"
|
||||
width="500"
|
||||
v-model="dialog"
|
||||
width="500"
|
||||
>
|
||||
<v-card
|
||||
color="primary"
|
||||
dark
|
||||
color="primary"
|
||||
dark
|
||||
>
|
||||
<v-card-title>
|
||||
Current resolution not calibrated
|
||||
@@ -230,9 +230,9 @@
|
||||
<v-card-text>
|
||||
Because the current resolution {{ this.$store.getters.currentVideoFormat.width }} x {{ this.$store.getters.currentVideoFormat.height }} is not yet calibrated, 3D mode cannot be enabled. Please
|
||||
<a
|
||||
href="/#/cameras"
|
||||
class="white--text"
|
||||
@click="$emit('switch-to-cameras')"
|
||||
href="/#/cameras"
|
||||
class="white--text"
|
||||
@click="$emit('switch-to-cameras')"
|
||||
> visit the Cameras tab</a> to calibrate this resolution. For now, SolvePNP will do nothing.
|
||||
</v-card-text>
|
||||
|
||||
@@ -241,9 +241,9 @@
|
||||
<v-card-actions>
|
||||
<v-spacer />
|
||||
<v-btn
|
||||
color="white"
|
||||
text
|
||||
@click="closeUncalibratedDialog"
|
||||
color="white"
|
||||
text
|
||||
@click="closeUncalibratedDialog"
|
||||
>
|
||||
OK
|
||||
</v-btn>
|
||||
@@ -264,252 +264,260 @@ import TargetsTab from "./PipelineViews/TargetsTab";
|
||||
import Map3DTab from './PipelineViews/Map3DTab';
|
||||
import PnPTab from './PipelineViews/PnPTab';
|
||||
import AprilTagTab from './PipelineViews/AprilTagTab';
|
||||
import ArucoTab from './PipelineViews/ArucoTab';
|
||||
|
||||
export default {
|
||||
name: 'Pipeline',
|
||||
components: {
|
||||
CameraAndPipelineSelect,
|
||||
cvImage,
|
||||
InputTab,
|
||||
ThresholdTab,
|
||||
ContoursTab,
|
||||
OutputTab,
|
||||
TargetsTab,
|
||||
Map3DTab,
|
||||
PnPTab,
|
||||
AprilTagTab,
|
||||
},
|
||||
data() {
|
||||
return {
|
||||
selectedTabsData: [0, 0, 0, 0],
|
||||
counterData: 0,
|
||||
dialog: false,
|
||||
processingModeOverride: false,
|
||||
hideNTWarning: false,
|
||||
}
|
||||
},
|
||||
computed: {
|
||||
selectedTabs: {
|
||||
get() {
|
||||
return this.$store.getters.isDriverMode ? [0] : this.selectedTabsData;
|
||||
},
|
||||
set(value) {
|
||||
this.selectedTabsData = value;
|
||||
}
|
||||
},
|
||||
tabGroups: {
|
||||
get() {
|
||||
let tabs = {
|
||||
input: {
|
||||
name: "Input",
|
||||
component: "InputTab",
|
||||
},
|
||||
threshold: {
|
||||
name: "Threshold",
|
||||
component: "ThresholdTab",
|
||||
},
|
||||
contours: {
|
||||
name: "Contours",
|
||||
component: "ContoursTab",
|
||||
},
|
||||
apriltag: {
|
||||
name: "AprilTag",
|
||||
component: "AprilTagTab",
|
||||
},
|
||||
output: {
|
||||
name: "Output",
|
||||
component: "OutputTab",
|
||||
},
|
||||
targets: {
|
||||
name: "Targets",
|
||||
component: "TargetsTab",
|
||||
},
|
||||
pnp: {
|
||||
name: "PnP",
|
||||
component: "PnPTab",
|
||||
},
|
||||
map3d: {
|
||||
name: "3D",
|
||||
component: "Map3DTab",
|
||||
}
|
||||
};
|
||||
|
||||
// If not in 3d, name "3D" is illegal
|
||||
const allow3d = this.$store.getters.currentPipelineSettings.solvePNPEnabled;
|
||||
// If in apriltag, "Threshold" and "Contours" are illegal -- otherwise "AprilTag" is
|
||||
const isAprilTag = (this.$store.getters.currentPipelineSettings.pipelineType - 2) === 2;
|
||||
|
||||
// 2D array of tab names and component names; each sub-array is a separate tab group
|
||||
let ret = [];
|
||||
if (this.$vuetify.breakpoint.smAndDown || this.$store.getters.isDriverMode || (this.$vuetify.breakpoint.mdAndDown && !this.$store.state.compactMode)) {
|
||||
// One big tab group with all the tabs
|
||||
ret[0] = Object.values(tabs);
|
||||
} else if (this.$vuetify.breakpoint.mdAndDown || !this.$store.state.compactMode) {
|
||||
// Two tab groups, one with "input, threshold, contours, output" and the other with "target info, 3D"
|
||||
ret[0] = [tabs.input, tabs.threshold, tabs.contours, tabs.apriltag, tabs.output];
|
||||
ret[1] = [tabs.targets, tabs.pnp, tabs.map3d];
|
||||
} else if (this.$vuetify.breakpoint.lgAndDown) {
|
||||
// Three tab groups, one with "input", one with "threshold, contours, output", and the other with "target info, 3D"
|
||||
ret[0] = [tabs.input];
|
||||
ret[1] = [tabs.threshold, tabs.contours, tabs.apriltag, tabs.output];
|
||||
ret[2] = [tabs.targets, tabs.pnp, tabs.map3d];
|
||||
} else if (this.$vuetify.breakpoint.xl) {
|
||||
// Three tab groups, one with "input", one with "threshold, contours", and the other with "output, target info, 3D"
|
||||
ret[0] = [tabs.input];
|
||||
ret[1] = [tabs.threshold];
|
||||
ret[2] = [tabs.contours, tabs.apriltag, tabs.output];
|
||||
ret[3] = [tabs.targets, tabs.pnp, tabs.map3d];
|
||||
}
|
||||
|
||||
for(let i = 0; i < ret.length; i++) {
|
||||
const group = ret[i];
|
||||
|
||||
// All the tabs we allow
|
||||
const filteredGroup = group.filter(it =>
|
||||
!(!allow3d && it.name === "3D") //Filter out 3D tab any time 3D isn't calibrated
|
||||
&& !((!allow3d || isAprilTag) && it.name === "PnP") //Filter out the PnP config tab if 3D isn't available, or we're doing Apriltags
|
||||
&& !(isAprilTag && (it.name === "Threshold")) //Filter out threshold tab if we're doing apriltags
|
||||
&& !(isAprilTag && (it.name === "Contours")) //Filter out contours if we're doing Apriltag
|
||||
&& !(!isAprilTag && it.name === "AprilTag") //Filter out apriltag unless we actually are doing Apriltags
|
||||
);
|
||||
ret[i] = filteredGroup;
|
||||
}
|
||||
|
||||
// One last filter to remove empty lists
|
||||
return ret.filter(it => it !== undefined && it.length > 0);
|
||||
}
|
||||
},
|
||||
processingMode: {
|
||||
get() {
|
||||
return (this.$store.getters.currentPipelineSettings.solvePNPEnabled || this.processingModeOverride) ? 1 : 0;
|
||||
},
|
||||
set(value) {
|
||||
if (this.$store.getters.isCalibrated) {
|
||||
this.$store.getters.currentPipelineSettings.solvePNPEnabled = value === 1;
|
||||
this.handlePipelineUpdate("solvePNPEnabled", value === 1);
|
||||
}
|
||||
}
|
||||
},
|
||||
driverMode: {
|
||||
get() {
|
||||
return this.$store.getters.isDriverMode;
|
||||
},
|
||||
set(value) {
|
||||
this.$store.getters.currentCameraSettings.currentPipelineIndex = value ? -1 : 0;
|
||||
this.handleInputWithIndex('currentPipeline', value ? -1 : 0);
|
||||
}
|
||||
},
|
||||
selectedOutputs: {
|
||||
// All this logic exists to deal with the reality that the output select buttons sometimes need an array and sometimes need a number (depending on whether or not they're exclusive)
|
||||
get() {
|
||||
// We switch the selector to single-select only on sm-and-down size devices, so we have to return a Number instead of an Array in that state
|
||||
let ret = [];
|
||||
if (this.$store.state.colorPicking) {
|
||||
ret = [0]; // We want the input stream only while color picking
|
||||
} else if (this.$store.getters.isDriverMode) {
|
||||
ret = [1]; // We want only the output stream in driver mode
|
||||
} else {
|
||||
if (this.$store.getters.currentPipelineSettings.inputShouldShow) ret = ret.concat([0]);
|
||||
if (this.$store.getters.currentPipelineSettings.outputShouldShow) ret = ret.concat([1]);
|
||||
if (!ret.length) ret = [0];
|
||||
}
|
||||
|
||||
if (this.$vuetify.breakpoint.mdAndUp) {
|
||||
return ret;
|
||||
} else {
|
||||
return ret[0] || 0;
|
||||
}
|
||||
},
|
||||
set(value) {
|
||||
let valToCommit = [0];
|
||||
if (value instanceof Array) {
|
||||
// Value is already an array, we don't need to do anything
|
||||
valToCommit = value;
|
||||
} else if (value) {
|
||||
// Value is assumed to be a number, so we wrap it into an array
|
||||
valToCommit = [value];
|
||||
}
|
||||
|
||||
this.$store.commit("mutatePipeline", {"inputShouldShow": valToCommit.includes(0)});
|
||||
this.$store.commit("mutatePipeline", {"outputShouldShow": valToCommit.includes(1)});
|
||||
this.handlePipelineUpdate("inputShouldShow", valToCommit.includes(0));
|
||||
}
|
||||
},
|
||||
fpsTooLow: {
|
||||
get() {
|
||||
// For now we only show the FPS is too low warning when GPU acceleration is enabled, because we don't really trust the presented video modes otherwise
|
||||
return this.$store.state.pipelineResults.fps - this.$store.getters.currentVideoFormat.fps < -5 && this.$store.state.pipelineResults.fps !== 0 && !this.$store.getters.isDriverMode && this.$store.state.settings.general.gpuAcceleration;
|
||||
}
|
||||
},
|
||||
latency: {
|
||||
get() {
|
||||
return this.$store.getters.currentPipelineResults.latency;
|
||||
}
|
||||
},
|
||||
isCalibrated: {
|
||||
get() {
|
||||
const resolution = this.$store.getters.videoFormatList[this.$store.getters.currentPipelineSettings.cameraVideoModeIndex];
|
||||
return this.$store.getters.currentCameraSettings.calibrations
|
||||
.some(e => e.width === resolution.width && e.height === resolution.height)
|
||||
}
|
||||
},
|
||||
isRobotConnected: {
|
||||
get() {
|
||||
// return this.$store.state.ntConnectionInfo.connected && this.$store.state.backendConnected;
|
||||
return true;
|
||||
}
|
||||
},
|
||||
showNTWarning: {
|
||||
get() {
|
||||
return (!this.$store.state.ntConnectionInfo.connected || this.$store.state.settings.networkSettings.runNTServer) && this.$store.state.settings.networkSettings.teamNumber > 0 && this.$store.state.backendConnected && !this.hideNTWarning;
|
||||
}
|
||||
},
|
||||
},
|
||||
created() {
|
||||
this.$store.state.connectedCallbacks.push(this.reloadStreams)
|
||||
},
|
||||
methods: {
|
||||
reloadStreams() {
|
||||
// Reload the streams as we technically close and reopen them
|
||||
this.$refs.streams.forEach(it => it.reload())
|
||||
},
|
||||
onImageClick(event) {
|
||||
// Only run on the input stream
|
||||
if (event.target.alt !== "Stream0") return;
|
||||
// Get a reference to the threshold tab (if it is shown) and call its "onClick" method
|
||||
let ref = this.$refs["Threshold"];
|
||||
if (ref && ref[0])
|
||||
ref[0].onClick(event)
|
||||
},
|
||||
on3DClick() {
|
||||
if (!this.$store.getters.isCalibrated) {
|
||||
this.dialog = true;
|
||||
this.processingModeOverride = true;
|
||||
}
|
||||
},
|
||||
closeUncalibratedDialog() {
|
||||
this.dialog = false;
|
||||
this.processingModeOverride = false;
|
||||
// this.$store.getters.currentPipelineSettings.solvePNPEnabled = false;
|
||||
this.handlePipelineUpdate("solvePNPEnabled", false);
|
||||
}
|
||||
name: 'Pipeline',
|
||||
components: {
|
||||
CameraAndPipelineSelect,
|
||||
cvImage,
|
||||
InputTab,
|
||||
ThresholdTab,
|
||||
ContoursTab,
|
||||
OutputTab,
|
||||
TargetsTab,
|
||||
Map3DTab,
|
||||
PnPTab,
|
||||
AprilTagTab,
|
||||
ArucoTab,
|
||||
},
|
||||
data() {
|
||||
return {
|
||||
selectedTabsData: [0, 0, 0, 0],
|
||||
counterData: 0,
|
||||
dialog: false,
|
||||
processingModeOverride: false,
|
||||
hideNTWarning: false,
|
||||
}
|
||||
},
|
||||
computed: {
|
||||
selectedTabs: {
|
||||
get() {
|
||||
return this.$store.getters.isDriverMode ? [0] : this.selectedTabsData;
|
||||
},
|
||||
set(value) {
|
||||
this.selectedTabsData = value;
|
||||
}
|
||||
},
|
||||
tabGroups: {
|
||||
get() {
|
||||
let tabs = {
|
||||
input: {
|
||||
name: "Input",
|
||||
component: "InputTab",
|
||||
},
|
||||
threshold: {
|
||||
name: "Threshold",
|
||||
component: "ThresholdTab",
|
||||
},
|
||||
contours: {
|
||||
name: "Contours",
|
||||
component: "ContoursTab",
|
||||
},
|
||||
apriltag: {
|
||||
name: "AprilTag",
|
||||
component: "AprilTagTab",
|
||||
},
|
||||
aruco: {
|
||||
name: "Aruco",
|
||||
component: "ArucoTab",
|
||||
},
|
||||
output: {
|
||||
name: "Output",
|
||||
component: "OutputTab",
|
||||
},
|
||||
targets: {
|
||||
name: "Targets",
|
||||
component: "TargetsTab",
|
||||
},
|
||||
pnp: {
|
||||
name: "PnP",
|
||||
component: "PnPTab",
|
||||
},
|
||||
map3d: {
|
||||
name: "3D",
|
||||
component: "Map3DTab",
|
||||
}
|
||||
};
|
||||
|
||||
// If not in 3d, name "3D" is illegal
|
||||
const allow3d = this.$store.getters.currentPipelineSettings.solvePNPEnabled;
|
||||
// If in apriltag, "Threshold" and "Contours" are illegal -- otherwise "AprilTag" is
|
||||
const isAprilTag = (this.$store.getters.currentPipelineSettings.pipelineType - 2) === 2;
|
||||
const isAruco = (this.$store.getters.currentPipelineSettings.pipelineType - 2) === 3;
|
||||
|
||||
// 2D array of tab names and component names; each sub-array is a separate tab group
|
||||
let ret = [];
|
||||
if (this.$vuetify.breakpoint.smAndDown || this.$store.getters.isDriverMode || (this.$vuetify.breakpoint.mdAndDown && !this.$store.state.compactMode)) {
|
||||
// One big tab group with all the tabs
|
||||
ret[0] = Object.values(tabs);
|
||||
} else if (this.$vuetify.breakpoint.mdAndDown || !this.$store.state.compactMode) {
|
||||
// Two tab groups, one with "input, threshold, contours, output" and the other with "target info, 3D"
|
||||
ret[0] = [tabs.input, tabs.threshold, tabs.contours, tabs.apriltag, tabs.aruco, tabs.output];
|
||||
ret[1] = [tabs.targets, tabs.pnp, tabs.map3d];
|
||||
} else if (this.$vuetify.breakpoint.lgAndDown) {
|
||||
// Three tab groups, one with "input", one with "threshold, contours, output", and the other with "target info, 3D"
|
||||
ret[0] = [tabs.input];
|
||||
ret[1] = [tabs.threshold, tabs.contours, tabs.apriltag,tabs.aruco, tabs.output];
|
||||
ret[2] = [tabs.targets, tabs.pnp, tabs.map3d];
|
||||
} else if (this.$vuetify.breakpoint.xl) {
|
||||
// Three tab groups, one with "input", one with "threshold, contours", and the other with "output, target info, 3D"
|
||||
ret[0] = [tabs.input];
|
||||
ret[1] = [tabs.threshold];
|
||||
ret[2] = [tabs.contours, tabs.apriltag, tabs.aruco,tabs.output];
|
||||
ret[3] = [tabs.targets, tabs.pnp, tabs.map3d];
|
||||
}
|
||||
|
||||
for(let i = 0; i < ret.length; i++) {
|
||||
const group = ret[i];
|
||||
|
||||
// All the tabs we allow
|
||||
const filteredGroup = group.filter(it =>
|
||||
!(!allow3d && it.name === "3D") //Filter out 3D tab any time 3D isn't calibrated
|
||||
&& !((!allow3d || isAprilTag || isAruco) && it.name === "PnP") //Filter out the PnP config tab if 3D isn't available, or we're doing Apriltags
|
||||
&& !((isAprilTag || isAruco) && (it.name === "Threshold")) //Filter out threshold tab if we're doing apriltags
|
||||
&& !((isAprilTag || isAruco)&& (it.name === "Contours")) //Filter out contours if we're doing Apriltag
|
||||
&& !(!isAprilTag && it.name === "AprilTag") //Filter out apriltag unless we actually are doing Apriltags
|
||||
&& !(!isAruco && it.name === "Aruco")
|
||||
);
|
||||
ret[i] = filteredGroup;
|
||||
}
|
||||
|
||||
// One last filter to remove empty lists
|
||||
return ret.filter(it => it !== undefined && it.length > 0);
|
||||
}
|
||||
},
|
||||
processingMode: {
|
||||
get() {
|
||||
return (this.$store.getters.currentPipelineSettings.solvePNPEnabled || this.processingModeOverride) ? 1 : 0;
|
||||
},
|
||||
set(value) {
|
||||
if (this.$store.getters.isCalibrated) {
|
||||
this.$store.getters.currentPipelineSettings.solvePNPEnabled = value === 1;
|
||||
this.handlePipelineUpdate("solvePNPEnabled", value === 1);
|
||||
}
|
||||
}
|
||||
},
|
||||
driverMode: {
|
||||
get() {
|
||||
return this.$store.getters.isDriverMode;
|
||||
},
|
||||
set(value) {
|
||||
this.$store.getters.currentCameraSettings.currentPipelineIndex = value ? -1 : 0;
|
||||
this.handleInputWithIndex('currentPipeline', value ? -1 : 0);
|
||||
}
|
||||
},
|
||||
selectedOutputs: {
|
||||
// All this logic exists to deal with the reality that the output select buttons sometimes need an array and sometimes need a number (depending on whether or not they're exclusive)
|
||||
get() {
|
||||
// We switch the selector to single-select only on sm-and-down size devices, so we have to return a Number instead of an Array in that state
|
||||
let ret = [];
|
||||
if (this.$store.state.colorPicking) {
|
||||
ret = [0]; // We want the input stream only while color picking
|
||||
} else if (this.$store.getters.isDriverMode) {
|
||||
ret = [1]; // We want only the output stream in driver mode
|
||||
} else {
|
||||
if (this.$store.getters.currentPipelineSettings.inputShouldShow) ret = ret.concat([0]);
|
||||
if (this.$store.getters.currentPipelineSettings.outputShouldShow) ret = ret.concat([1]);
|
||||
if (!ret.length) ret = [0];
|
||||
}
|
||||
|
||||
if (this.$vuetify.breakpoint.mdAndUp) {
|
||||
return ret;
|
||||
} else {
|
||||
return ret[0] || 0;
|
||||
}
|
||||
},
|
||||
set(value) {
|
||||
let valToCommit = [0];
|
||||
if (value instanceof Array) {
|
||||
// Value is already an array, we don't need to do anything
|
||||
valToCommit = value;
|
||||
} else if (value) {
|
||||
// Value is assumed to be a number, so we wrap it into an array
|
||||
valToCommit = [value];
|
||||
}
|
||||
|
||||
this.$store.commit("mutatePipeline", {"inputShouldShow": valToCommit.includes(0)});
|
||||
this.$store.commit("mutatePipeline", {"outputShouldShow": valToCommit.includes(1)});
|
||||
this.handlePipelineUpdate("inputShouldShow", valToCommit.includes(0));
|
||||
}
|
||||
},
|
||||
fpsTooLow: {
|
||||
get() {
|
||||
// For now we only show the FPS is too low warning when GPU acceleration is enabled, because we don't really trust the presented video modes otherwise
|
||||
return this.$store.state.pipelineResults.fps - this.$store.getters.currentVideoFormat.fps < -5 && this.$store.state.pipelineResults.fps !== 0 && !this.$store.getters.isDriverMode && this.$store.state.settings.general.gpuAcceleration;
|
||||
}
|
||||
},
|
||||
latency: {
|
||||
get() {
|
||||
return this.$store.getters.currentPipelineResults.latency;
|
||||
}
|
||||
},
|
||||
isCalibrated: {
|
||||
get() {
|
||||
const resolution = this.$store.getters.videoFormatList[this.$store.getters.currentPipelineSettings.cameraVideoModeIndex];
|
||||
return this.$store.getters.currentCameraSettings.calibrations
|
||||
.some(e => e.width === resolution.width && e.height === resolution.height)
|
||||
}
|
||||
},
|
||||
isRobotConnected: {
|
||||
get() {
|
||||
// return this.$store.state.ntConnectionInfo.connected && this.$store.state.backendConnected;
|
||||
return true;
|
||||
}
|
||||
},
|
||||
showNTWarning: {
|
||||
get() {
|
||||
return (!this.$store.state.ntConnectionInfo.connected || this.$store.state.settings.networkSettings.runNTServer) && this.$store.state.settings.networkSettings.teamNumber > 0 && this.$store.state.backendConnected && !this.hideNTWarning;
|
||||
}
|
||||
},
|
||||
},
|
||||
created() {
|
||||
this.$store.state.connectedCallbacks.push(this.reloadStreams)
|
||||
},
|
||||
methods: {
|
||||
reloadStreams() {
|
||||
// Reload the streams as we technically close and reopen them
|
||||
this.$refs.streams.forEach(it => it.reload())
|
||||
},
|
||||
onImageClick(event) {
|
||||
// Only run on the input stream
|
||||
if (event.target.alt !== "Stream0") return;
|
||||
// Get a reference to the threshold tab (if it is shown) and call its "onClick" method
|
||||
let ref = this.$refs["Threshold"];
|
||||
if (ref && ref[0])
|
||||
ref[0].onClick(event)
|
||||
},
|
||||
on3DClick() {
|
||||
if (!this.$store.getters.isCalibrated) {
|
||||
this.dialog = true;
|
||||
this.processingModeOverride = true;
|
||||
}
|
||||
},
|
||||
closeUncalibratedDialog() {
|
||||
this.dialog = false;
|
||||
this.processingModeOverride = false;
|
||||
// this.$store.getters.currentPipelineSettings.solvePNPEnabled = false;
|
||||
this.handlePipelineUpdate("solvePNPEnabled", false);
|
||||
}
|
||||
}
|
||||
}
|
||||
</script>
|
||||
|
||||
<style scoped>
|
||||
.v-btn-toggle.fill {
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
}
|
||||
|
||||
.v-btn-toggle.fill > .v-btn {
|
||||
width: 50%;
|
||||
height: 100%;
|
||||
width: 50%;
|
||||
height: 100%;
|
||||
}
|
||||
|
||||
th {
|
||||
width: 80px;
|
||||
text-align: center;
|
||||
width: 80px;
|
||||
text-align: center;
|
||||
}
|
||||
</style>
|
||||
|
||||
76
photon-client/src/views/PipelineViews/ArucoTab.vue
Normal file
76
photon-client/src/views/PipelineViews/ArucoTab.vue
Normal file
@@ -0,0 +1,76 @@
|
||||
<template>
|
||||
<div>
|
||||
<CVslider
|
||||
v-model="decimate"
|
||||
class="pt-2"
|
||||
slider-cols="8"
|
||||
name="Decimate"
|
||||
min="1"
|
||||
max="8"
|
||||
step=".5"
|
||||
tooltip="Increases FPS at the expense of range by reducing image resolution initially"
|
||||
@input="handlePipelineData('decimate')"
|
||||
/>
|
||||
<CVslider
|
||||
v-model="numIterations"
|
||||
class="pt-2"
|
||||
slider-cols="8"
|
||||
name="Corner Iterations"
|
||||
min="30"
|
||||
max="1000"
|
||||
step="5"
|
||||
tooltip="How many iterations are going to be used in order to refine corners. Higher values are lead to more accuracy at the cost of performance"
|
||||
@input="handlePipelineData('numIterations')"
|
||||
/>
|
||||
<CVslider
|
||||
v-model="cornerAccuracy"
|
||||
class="pt-2"
|
||||
slider-cols="8"
|
||||
name="Corner Accuracy"
|
||||
min=".01"
|
||||
max="100"
|
||||
step=".01"
|
||||
tooltip="Minimum accuracy for the corners, lower is better but more performance intensive "
|
||||
@input="handlePipelineData('cornerAccuracy')"
|
||||
/>
|
||||
</div>
|
||||
</template>
|
||||
|
||||
<script>
|
||||
import CVslider from '../../components/common/cv-slider'
|
||||
|
||||
export default {
|
||||
name: "Aruco",
|
||||
components: {
|
||||
CVslider
|
||||
},
|
||||
computed: {
|
||||
decimate: {
|
||||
get() {
|
||||
return this.$store.getters.currentPipelineSettings.decimate
|
||||
},
|
||||
set(val) {
|
||||
this.$store.commit("mutatePipeline", {"decimate": val});
|
||||
},
|
||||
},
|
||||
numIterations: {
|
||||
get() {
|
||||
return this.$store.getters.currentPipelineSettings.numIterations
|
||||
},
|
||||
set(val) {
|
||||
this.$store.commit("mutatePipeline", {"numIterations": val});
|
||||
},
|
||||
},
|
||||
cornerAccuracy: {
|
||||
get() {
|
||||
return this.$store.getters.currentPipelineSettings.cornerAccuracy
|
||||
},
|
||||
set(val) {
|
||||
this.$store.commit("mutatePipeline", {"cornerAccuracy": val});
|
||||
},
|
||||
},
|
||||
},
|
||||
methods: {
|
||||
}
|
||||
}
|
||||
</script>
|
||||
@@ -19,7 +19,7 @@
|
||||
Target
|
||||
</th>
|
||||
<th
|
||||
v-if="$store.getters.pipelineType === 4"
|
||||
v-if="$store.getters.pipelineType === 4 || (($store.getters.pipelineType - 2) === 3)"
|
||||
class="text-center"
|
||||
>
|
||||
Fiducial ID
|
||||
@@ -62,7 +62,7 @@
|
||||
:key="index"
|
||||
>
|
||||
<td>{{ index }}</td>
|
||||
<td v-if="$store.getters.pipelineType === 4">
|
||||
<td v-if="$store.getters.pipelineType === 4 || (($store.getters.pipelineType - 2) === 3)">
|
||||
{{ parseInt(value.fiducialId) }}
|
||||
</td>
|
||||
<template v-if="!$store.getters.currentPipelineSettings.solvePNPEnabled">
|
||||
|
||||
@@ -197,6 +197,7 @@ public class TestUtils {
|
||||
public enum ApriltagTestImages {
|
||||
kRobots,
|
||||
kTag1_640_480,
|
||||
kTag1_16h5_1280,
|
||||
kTag_corner_1280;
|
||||
|
||||
public final Path path;
|
||||
@@ -204,7 +205,9 @@ public class TestUtils {
|
||||
Path getPath() {
|
||||
// Strip leading k
|
||||
var filename = this.toString().substring(1).toLowerCase();
|
||||
return Path.of("apriltag", filename + ".jpg");
|
||||
var extension = ".jpg";
|
||||
if (filename.equals("tag1_16h5_1280")) extension = ".png";
|
||||
return Path.of("apriltag", filename + extension);
|
||||
}
|
||||
|
||||
ApriltagTestImages() {
|
||||
|
||||
@@ -172,6 +172,17 @@ public class MathUtils {
|
||||
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
|
||||
@@ -208,6 +219,14 @@ public class MathUtils {
|
||||
return new Transform3d(pose.getTranslation(), ocvRotation);
|
||||
}
|
||||
|
||||
public static Pose3d convertArucotoOpenCV(Transform3d pose) {
|
||||
var ocvRotation =
|
||||
APRILTAG_BASE_ROTATION.rotateBy(
|
||||
new Rotation3d(VecBuilder.fill(0, 0, 1), Units.degreesToRadians(180))
|
||||
.rotateBy(pose.getRotation()));
|
||||
return new Pose3d(pose.getTranslation(), ocvRotation);
|
||||
}
|
||||
|
||||
public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) {
|
||||
var angle = rotation.getAngle();
|
||||
var axis = rotation.getAxis().times(angle);
|
||||
|
||||
@@ -0,0 +1,83 @@
|
||||
/*
|
||||
* 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.aruco;
|
||||
|
||||
import java.util.Arrays;
|
||||
import org.photonvision.common.logging.LogGroup;
|
||||
import org.photonvision.common.logging.Logger;
|
||||
|
||||
public class ArucoDetectionResult {
|
||||
private static final Logger logger =
|
||||
new Logger(ArucoDetectionResult.class, LogGroup.VisionModule);
|
||||
double[] xCorners;
|
||||
double[] yCorners;
|
||||
|
||||
int id;
|
||||
|
||||
double[] tvec, rvec;
|
||||
|
||||
public ArucoDetectionResult(
|
||||
double[] xCorners, double[] yCorners, int id, double[] tvec, double[] rvec) {
|
||||
this.xCorners = xCorners;
|
||||
this.yCorners = yCorners;
|
||||
this.id = id;
|
||||
this.tvec = tvec;
|
||||
this.rvec = rvec;
|
||||
// logger.debug("Creating a new detection result: " + this.toString());
|
||||
}
|
||||
|
||||
public double[] getTvec() {
|
||||
return tvec;
|
||||
}
|
||||
|
||||
public double[] getRvec() {
|
||||
return rvec;
|
||||
}
|
||||
|
||||
public double[] getxCorners() {
|
||||
return xCorners;
|
||||
}
|
||||
|
||||
public double[] getyCorners() {
|
||||
return yCorners;
|
||||
}
|
||||
|
||||
public int getId() {
|
||||
return id;
|
||||
}
|
||||
|
||||
public double getCenterX() {
|
||||
return (xCorners[0] + xCorners[1] + xCorners[2] + xCorners[3]) * .25;
|
||||
}
|
||||
|
||||
public double getCenterY() {
|
||||
return (yCorners[0] + yCorners[1] + yCorners[2] + yCorners[3]) * .25;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return "ArucoDetectionResult{"
|
||||
+ "xCorners="
|
||||
+ Arrays.toString(xCorners)
|
||||
+ ", yCorners="
|
||||
+ Arrays.toString(yCorners)
|
||||
+ ", id="
|
||||
+ id
|
||||
+ '}';
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
/*
|
||||
* 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.aruco;
|
||||
|
||||
import org.opencv.aruco.Aruco;
|
||||
import org.opencv.aruco.ArucoDetector;
|
||||
import org.opencv.aruco.DetectorParameters;
|
||||
import org.opencv.aruco.Dictionary;
|
||||
import org.photonvision.common.logging.LogGroup;
|
||||
import org.photonvision.common.logging.Logger;
|
||||
|
||||
public class ArucoDetectorParams {
|
||||
private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
|
||||
|
||||
private float m_decimate = -1;
|
||||
private int m_iterations = -1;
|
||||
private double m_accuracy = -1;
|
||||
|
||||
DetectorParameters parameters = DetectorParameters.create();
|
||||
ArucoDetector detector;
|
||||
|
||||
public ArucoDetectorParams() {
|
||||
setDecimation(1);
|
||||
setCornerAccuracy(25);
|
||||
setCornerRefinementMaxIterations(100);
|
||||
|
||||
detector = new ArucoDetector(Dictionary.get(Aruco.DICT_APRILTAG_16h5), parameters);
|
||||
}
|
||||
|
||||
public void setDecimation(float decimate) {
|
||||
if (decimate == m_decimate) return;
|
||||
|
||||
logger.info("Setting decimation from " + m_decimate + " to " + decimate);
|
||||
|
||||
// We only need to mutate the parameters -- the detector keeps a poitner to the parameters
|
||||
// object internally, so it should automatically update
|
||||
parameters.set_aprilTagQuadDecimate((float) decimate);
|
||||
m_decimate = decimate;
|
||||
}
|
||||
|
||||
public void setCornerRefinementMaxIterations(int iters) {
|
||||
if (iters == m_iterations || iters <= 0) return;
|
||||
|
||||
parameters.set_cornerRefinementMethod(Aruco.CORNER_REFINE_SUBPIX);
|
||||
parameters.set_cornerRefinementMaxIterations(iters); // 200
|
||||
|
||||
m_iterations = iters;
|
||||
}
|
||||
|
||||
public void setCornerAccuracy(double accuracy) {
|
||||
if (accuracy == m_accuracy || accuracy <= 0) return;
|
||||
|
||||
parameters.set_cornerRefinementMinAccuracy(
|
||||
accuracy / 1000.0); // divides by 1000 because the UI multiplies it by 1000
|
||||
m_accuracy = accuracy;
|
||||
}
|
||||
|
||||
public ArucoDetector getDetector() {
|
||||
return detector;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,132 @@
|
||||
/*
|
||||
* 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.aruco;
|
||||
|
||||
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.Translation3d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import java.util.ArrayList;
|
||||
import org.opencv.aruco.Aruco;
|
||||
import org.opencv.aruco.ArucoDetector;
|
||||
import org.opencv.core.Mat;
|
||||
import org.photonvision.common.logging.LogGroup;
|
||||
import org.photonvision.common.logging.Logger;
|
||||
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
|
||||
|
||||
public class PhotonArucoDetector {
|
||||
private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
|
||||
|
||||
private static final Rotation3d ARUCO_BASE_ROTATION =
|
||||
new Rotation3d(VecBuilder.fill(0, 0, 1), Units.degreesToRadians(180));
|
||||
|
||||
Mat ids;
|
||||
|
||||
Mat tvecs;
|
||||
Mat rvecs;
|
||||
ArrayList<Mat> corners;
|
||||
|
||||
Mat cornerMat;
|
||||
Translation3d translation;
|
||||
Rotation3d rotation;
|
||||
double timeStartDetect;
|
||||
double timeEndDetect;
|
||||
Pose3d tagPose;
|
||||
double timeStartProcess;
|
||||
double timeEndProcess;
|
||||
double[] xCorners = new double[4];
|
||||
double[] yCorners = new double[4];
|
||||
|
||||
public PhotonArucoDetector() {
|
||||
logger.debug("New Aruco Detector");
|
||||
ids = new Mat();
|
||||
tvecs = new Mat();
|
||||
rvecs = new Mat();
|
||||
corners = new ArrayList<Mat>();
|
||||
tagPose = new Pose3d();
|
||||
translation = new Translation3d();
|
||||
rotation = new Rotation3d();
|
||||
}
|
||||
|
||||
public ArucoDetectionResult[] detect(
|
||||
Mat grayscaleImg,
|
||||
float tagSize,
|
||||
CameraCalibrationCoefficients coeffs,
|
||||
ArucoDetector detector) {
|
||||
detector.detectMarkers(grayscaleImg, corners, ids);
|
||||
if (coeffs != null) {
|
||||
Aruco.estimatePoseSingleMarkers(
|
||||
corners,
|
||||
tagSize,
|
||||
coeffs.getCameraIntrinsicsMat(),
|
||||
coeffs.getDistCoeffsMat(),
|
||||
rvecs,
|
||||
tvecs);
|
||||
}
|
||||
|
||||
ArucoDetectionResult[] toReturn = new ArucoDetectionResult[corners.size()];
|
||||
timeStartProcess = System.currentTimeMillis();
|
||||
for (int i = 0; i < corners.size(); i++) {
|
||||
cornerMat = corners.get(i);
|
||||
// logger.debug(cornerMat.dump());
|
||||
xCorners =
|
||||
new double[] {
|
||||
cornerMat.get(0, 0)[0],
|
||||
cornerMat.get(0, 1)[0],
|
||||
cornerMat.get(0, 2)[0],
|
||||
cornerMat.get(0, 3)[0]
|
||||
};
|
||||
yCorners =
|
||||
new double[] {
|
||||
cornerMat.get(0, 0)[1],
|
||||
cornerMat.get(0, 1)[1],
|
||||
cornerMat.get(0, 2)[1],
|
||||
cornerMat.get(0, 3)[1]
|
||||
};
|
||||
cornerMat.release();
|
||||
|
||||
double[] tvec;
|
||||
double[] rvec;
|
||||
if (coeffs != null) {
|
||||
// Need to apply a 180 rotation about Z
|
||||
var origRvec = rvecs.get(i, 0);
|
||||
var axisangle = VecBuilder.fill(origRvec[0], origRvec[1], origRvec[2]);
|
||||
Rotation3d rotation = new Rotation3d(axisangle, axisangle.normF());
|
||||
var ocvRotation = ARUCO_BASE_ROTATION.rotateBy(rotation);
|
||||
|
||||
var angle = ocvRotation.getAngle();
|
||||
var finalAxisAngle = ocvRotation.getAxis().times(angle);
|
||||
|
||||
tvec = tvecs.get(i, 0);
|
||||
rvec = finalAxisAngle.getData();
|
||||
} else {
|
||||
tvec = new double[] {0, 0, 0};
|
||||
rvec = new double[] {0, 0, 0};
|
||||
}
|
||||
|
||||
toReturn[i] =
|
||||
new ArucoDetectionResult(xCorners, yCorners, (int) ids.get(i, 0)[0], tvec, rvec);
|
||||
}
|
||||
rvecs.release();
|
||||
tvecs.release();
|
||||
ids.release();
|
||||
|
||||
return toReturn;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
/*
|
||||
* 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.math.util.Units;
|
||||
import java.util.List;
|
||||
import org.opencv.aruco.DetectorParameters;
|
||||
import org.opencv.core.Mat;
|
||||
import org.photonvision.vision.aruco.ArucoDetectionResult;
|
||||
import org.photonvision.vision.aruco.PhotonArucoDetector;
|
||||
import org.photonvision.vision.pipe.CVPipe;
|
||||
|
||||
public class ArucoDetectionPipe
|
||||
extends CVPipe<Mat, List<ArucoDetectionResult>, ArucoDetectionPipeParams> {
|
||||
PhotonArucoDetector detector = new PhotonArucoDetector();
|
||||
|
||||
@Override
|
||||
protected List<ArucoDetectionResult> process(Mat in) {
|
||||
return List.of(
|
||||
detector.detect(
|
||||
in,
|
||||
(float) Units.inchesToMeters(6),
|
||||
params.cameraCalibrationCoefficients,
|
||||
params.detectorParams));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setParams(ArucoDetectionPipeParams params) {
|
||||
super.setParams(params);
|
||||
}
|
||||
|
||||
public DetectorParameters getParameters() {
|
||||
return params == null ? null : params.detectorParams.get_params();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
/*
|
||||
* 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 java.util.Objects;
|
||||
import org.opencv.aruco.ArucoDetector;
|
||||
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
|
||||
|
||||
public class ArucoDetectionPipeParams {
|
||||
public ArucoDetector detectorParams;
|
||||
public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
|
||||
|
||||
public ArucoDetectionPipeParams(
|
||||
ArucoDetector detector, CameraCalibrationCoefficients cameraCalibrationCoefficients) {
|
||||
this.detectorParams = detector;
|
||||
this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean equals(Object o) {
|
||||
if (this == o) return true;
|
||||
if (o == null || getClass() != o.getClass()) return false;
|
||||
ArucoDetectionPipeParams that = (ArucoDetectionPipeParams) o;
|
||||
return Objects.equals(detectorParams, that.detectorParams)
|
||||
&& Objects.equals(cameraCalibrationCoefficients, that.cameraCalibrationCoefficients);
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(detectorParams, cameraCalibrationCoefficients);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return "ArucoDetectionPipeParams{"
|
||||
+ "detectorParams="
|
||||
+ detectorParams
|
||||
+ ", cameraCalibrationCoefficients="
|
||||
+ cameraCalibrationCoefficients
|
||||
+ '}';
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* 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 java.awt.*;
|
||||
import org.photonvision.vision.frame.FrameDivisor;
|
||||
|
||||
public class Draw2dArucoPipe extends Draw2dTargetsPipe {
|
||||
public static class Draw2dArucoParams extends Draw2dTargetsPipe.Draw2dTargetsParams {
|
||||
public Draw2dArucoParams(
|
||||
boolean shouldDraw, boolean showMultipleTargets, FrameDivisor divisor) {
|
||||
super(shouldDraw, showMultipleTargets, divisor);
|
||||
// We want to show the polygon, not the rotated box
|
||||
this.showRotatedBox = false;
|
||||
this.showMaximumBox = false;
|
||||
this.rotatedBoxColor = Color.RED;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
* 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 org.photonvision.vision.calibration.CameraCalibrationCoefficients;
|
||||
import org.photonvision.vision.frame.FrameDivisor;
|
||||
import org.photonvision.vision.target.TargetModel;
|
||||
|
||||
public class Draw3dArucoPipe extends Draw3dTargetsPipe {
|
||||
public static class Draw3dArucoParams extends Draw3dContoursParams {
|
||||
public Draw3dArucoParams(
|
||||
boolean shouldDraw,
|
||||
CameraCalibrationCoefficients cameraCalibrationCoefficients,
|
||||
TargetModel targetModel,
|
||||
FrameDivisor divisor) {
|
||||
super(shouldDraw, cameraCalibrationCoefficients, targetModel, divisor);
|
||||
this.shouldDrawHull = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* 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.pipeline;
|
||||
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import org.opencv.core.Mat;
|
||||
import org.photonvision.vision.aruco.ArucoDetectionResult;
|
||||
import org.photonvision.vision.aruco.ArucoDetectorParams;
|
||||
import org.photonvision.vision.frame.Frame;
|
||||
import org.photonvision.vision.frame.FrameThresholdType;
|
||||
import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
|
||||
import org.photonvision.vision.pipe.impl.*;
|
||||
import org.photonvision.vision.pipeline.result.CVPipelineResult;
|
||||
import org.photonvision.vision.target.TrackedTarget;
|
||||
import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
|
||||
|
||||
@SuppressWarnings("DuplicatedCode")
|
||||
public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSettings> {
|
||||
private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
|
||||
private final GrayscalePipe grayscalePipe = new GrayscalePipe();
|
||||
|
||||
private final ArucoDetectionPipe arucoDetectionPipe = new ArucoDetectionPipe();
|
||||
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
|
||||
|
||||
ArucoDetectorParams m_arucoDetectorParams = new ArucoDetectorParams();
|
||||
|
||||
public ArucoPipeline() {
|
||||
super(FrameThresholdType.GREYSCALE);
|
||||
settings = new ArucoPipelineSettings();
|
||||
}
|
||||
|
||||
public ArucoPipeline(ArucoPipelineSettings settings) {
|
||||
super(FrameThresholdType.GREYSCALE);
|
||||
this.settings = settings;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void setPipeParamsImpl() {
|
||||
// Sanitize thread count - not supported to have fewer than 1 threads
|
||||
settings.threads = Math.max(1, settings.threads);
|
||||
|
||||
RotateImagePipe.RotateImageParams rotateImageParams =
|
||||
new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
|
||||
rotateImagePipe.setParams(rotateImageParams);
|
||||
|
||||
m_arucoDetectorParams.setDecimation((float) settings.decimate);
|
||||
m_arucoDetectorParams.setCornerRefinementMaxIterations(settings.numIterations);
|
||||
m_arucoDetectorParams.setCornerAccuracy(settings.cornerAccuracy);
|
||||
|
||||
arucoDetectionPipe.setParams(
|
||||
new ArucoDetectionPipeParams(
|
||||
m_arucoDetectorParams.getDetector(), frameStaticProperties.cameraCalibration));
|
||||
}
|
||||
|
||||
@Override
|
||||
protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) {
|
||||
long sumPipeNanosElapsed = 0L;
|
||||
Mat rawInputMat;
|
||||
rawInputMat = frame.colorImage.getMat();
|
||||
|
||||
List<TrackedTarget> targetList;
|
||||
CVPipeResult<List<ArucoDetectionResult>> tagDetectionPipeResult;
|
||||
|
||||
if (rawInputMat.empty()) {
|
||||
return new CVPipelineResult(sumPipeNanosElapsed, 0, List.of(), frame);
|
||||
}
|
||||
|
||||
tagDetectionPipeResult = arucoDetectionPipe.run(rawInputMat);
|
||||
targetList = new ArrayList<>();
|
||||
for (ArucoDetectionResult detection : tagDetectionPipeResult.output) {
|
||||
// TODO this should be in a pipe, not in the top level here (Matt)
|
||||
|
||||
// populate the target list
|
||||
// Challenge here is that TrackedTarget functions with OpenCV Contour
|
||||
TrackedTarget target =
|
||||
new TrackedTarget(
|
||||
detection,
|
||||
new TargetCalculationParameters(
|
||||
false, null, null, null, null, frameStaticProperties));
|
||||
|
||||
var correctedBestPose = target.getBestCameraToTarget3d();
|
||||
|
||||
target.setBestCameraToTarget3d(
|
||||
new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
|
||||
|
||||
targetList.add(target);
|
||||
}
|
||||
|
||||
var fpsResult = calculateFPSPipe.run(null);
|
||||
var fps = fpsResult.output;
|
||||
|
||||
return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, frame);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
/*
|
||||
* 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.pipeline;
|
||||
|
||||
import com.fasterxml.jackson.annotation.JsonTypeName;
|
||||
import org.photonvision.vision.target.TargetModel;
|
||||
|
||||
@JsonTypeName("ArucoPipelineSettings")
|
||||
public class ArucoPipelineSettings extends AdvancedPipelineSettings {
|
||||
public double decimate = 1;
|
||||
public int threads = 2;
|
||||
public int numIterations = 100;
|
||||
public double cornerAccuracy = 25.0;
|
||||
public boolean useAruco3 = true;
|
||||
|
||||
// 3d settings
|
||||
|
||||
public ArucoPipelineSettings() {
|
||||
super();
|
||||
pipelineType = PipelineType.Aruco;
|
||||
outputShowMultipleTargets = true;
|
||||
targetModel = TargetModel.kAruco6in_16h5;
|
||||
cameraExposure = -1;
|
||||
cameraAutoExposure = true;
|
||||
ledMode = false;
|
||||
}
|
||||
}
|
||||
@@ -31,7 +31,8 @@ import org.photonvision.vision.opencv.ImageRotationMode;
|
||||
@JsonSubTypes.Type(value = ColoredShapePipelineSettings.class),
|
||||
@JsonSubTypes.Type(value = ReflectivePipelineSettings.class),
|
||||
@JsonSubTypes.Type(value = DriverModePipelineSettings.class),
|
||||
@JsonSubTypes.Type(value = AprilTagPipelineSettings.class)
|
||||
@JsonSubTypes.Type(value = AprilTagPipelineSettings.class),
|
||||
@JsonSubTypes.Type(value = ArucoPipelineSettings.class)
|
||||
})
|
||||
public class CVPipelineSettings implements Cloneable {
|
||||
public int pipelineIndex = 0;
|
||||
|
||||
@@ -24,6 +24,7 @@ import org.photonvision.vision.frame.FrameStaticProperties;
|
||||
import org.photonvision.vision.opencv.DualOffsetValues;
|
||||
import org.photonvision.vision.pipe.impl.*;
|
||||
import org.photonvision.vision.pipeline.result.CVPipelineResult;
|
||||
import org.photonvision.vision.target.TargetModel;
|
||||
import org.photonvision.vision.target.TrackedTarget;
|
||||
|
||||
/**
|
||||
@@ -37,6 +38,9 @@ public class OutputStreamPipeline {
|
||||
private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
|
||||
private final Draw2dAprilTagsPipe draw2dAprilTagsPipe = new Draw2dAprilTagsPipe();
|
||||
private final Draw3dAprilTagsPipe draw3dAprilTagsPipe = new Draw3dAprilTagsPipe();
|
||||
|
||||
private final Draw2dArucoPipe draw2dArucoPipe = new Draw2dArucoPipe();
|
||||
private final Draw3dArucoPipe draw3dArucoPipe = new Draw3dArucoPipe();
|
||||
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
|
||||
private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
|
||||
|
||||
@@ -65,6 +69,13 @@ public class OutputStreamPipeline {
|
||||
settings.streamingFrameDivisor);
|
||||
draw2dAprilTagsPipe.setParams(draw2DAprilTagsParams);
|
||||
|
||||
var draw2DArucoParams =
|
||||
new Draw2dArucoPipe.Draw2dArucoParams(
|
||||
settings.outputShouldDraw,
|
||||
settings.outputShowMultipleTargets,
|
||||
settings.streamingFrameDivisor);
|
||||
draw2dArucoPipe.setParams(draw2DArucoParams);
|
||||
|
||||
var draw2dCrosshairParams =
|
||||
new Draw2dCrosshairPipe.Draw2dCrosshairParams(
|
||||
settings.outputShouldDraw,
|
||||
@@ -92,6 +103,14 @@ public class OutputStreamPipeline {
|
||||
settings.streamingFrameDivisor);
|
||||
draw3dAprilTagsPipe.setParams(draw3dAprilTagsParams);
|
||||
|
||||
var draw3dArucoParams =
|
||||
new Draw3dArucoPipe.Draw3dArucoParams(
|
||||
settings.outputShouldDraw,
|
||||
frameStaticProperties.cameraCalibration,
|
||||
TargetModel.k6in_16h5,
|
||||
settings.streamingFrameDivisor);
|
||||
draw3dArucoPipe.setParams(draw3dArucoParams);
|
||||
|
||||
resizeImagePipe.setParams(
|
||||
new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
|
||||
}
|
||||
@@ -126,12 +145,12 @@ public class OutputStreamPipeline {
|
||||
}
|
||||
|
||||
// Draw 2D Crosshair on output
|
||||
var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
|
||||
var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
|
||||
|
||||
if (!(settings instanceof AprilTagPipelineSettings)) {
|
||||
// If we're processing anything other than Apriltags...
|
||||
|
||||
if (!(settings instanceof AprilTagPipelineSettings)
|
||||
&& !(settings instanceof ArucoPipelineSettings)) {
|
||||
// If we're processing anything other than Apriltags..
|
||||
var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
|
||||
|
||||
@@ -154,7 +173,7 @@ public class OutputStreamPipeline {
|
||||
pipeProfileNanos[8] = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
} else if (settings instanceof AprilTagPipelineSettings) {
|
||||
// If we are doing apriltags...
|
||||
if (settings.solvePNPEnabled) {
|
||||
// Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
|
||||
@@ -171,6 +190,26 @@ public class OutputStreamPipeline {
|
||||
var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
|
||||
|
||||
pipeProfileNanos[6] = 0;
|
||||
pipeProfileNanos[7] = 0;
|
||||
pipeProfileNanos[8] = 0;
|
||||
}
|
||||
} else if (settings instanceof ArucoPipelineSettings) {
|
||||
if (settings.solvePNPEnabled) {
|
||||
// Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
|
||||
pipeProfileNanos[5] = 0;
|
||||
pipeProfileNanos[6] = 0;
|
||||
|
||||
var drawOnInputResult = draw3dArucoPipe.run(Pair.of(outMat, targetsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
|
||||
|
||||
pipeProfileNanos[8] = 0;
|
||||
|
||||
} else {
|
||||
// Draw 2d apriltag markers
|
||||
var draw2dTargetsOnInput = draw2dArucoPipe.run(Pair.of(outMat, targetsToDraw));
|
||||
sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
|
||||
|
||||
pipeProfileNanos[6] = 0;
|
||||
pipeProfileNanos[7] = 0;
|
||||
pipeProfileNanos[8] = 0;
|
||||
|
||||
@@ -23,7 +23,8 @@ public enum PipelineType {
|
||||
DriverMode(-1, DriverModePipeline.class),
|
||||
Reflective(0, ReflectivePipeline.class),
|
||||
ColoredShape(1, ColoredShapePipeline.class),
|
||||
AprilTag(2, AprilTagPipeline.class);
|
||||
AprilTag(2, AprilTagPipeline.class),
|
||||
Aruco(3, ArucoPipeline.class);
|
||||
|
||||
public final int baseIndex;
|
||||
public final Class clazz;
|
||||
|
||||
@@ -21,6 +21,7 @@ import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.Comparator;
|
||||
import java.util.List;
|
||||
import org.opencv.aruco.Aruco;
|
||||
import org.photonvision.common.configuration.CameraConfiguration;
|
||||
import org.photonvision.common.configuration.ConfigManager;
|
||||
import org.photonvision.common.dataflow.DataChangeService;
|
||||
@@ -205,6 +206,11 @@ public class PipelineManager {
|
||||
currentUserPipeline =
|
||||
new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings);
|
||||
break;
|
||||
|
||||
case Aruco:
|
||||
logger.debug("Creating Aruco Pipeline");
|
||||
currentUserPipeline = new ArucoPipeline((ArucoPipelineSettings) desiredPipelineSettings);
|
||||
break;
|
||||
default:
|
||||
// Can be calib3d or drivermode, both of which are special cases
|
||||
break;
|
||||
@@ -300,6 +306,12 @@ public class PipelineManager {
|
||||
added.pipelineNickname = nickname;
|
||||
return added;
|
||||
}
|
||||
case Aruco:
|
||||
{
|
||||
var added = new ArucoPipelineSettings();
|
||||
added.pipelineNickname = nickname;
|
||||
return added;
|
||||
}
|
||||
default:
|
||||
{
|
||||
logger.error("Got invalid pipeline type: " + type.toString());
|
||||
|
||||
@@ -109,6 +109,14 @@ public enum TargetModel implements Releasable {
|
||||
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)),
|
||||
kAruco6in_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)),
|
||||
Units.inchesToMeters(3 * 2)),
|
||||
k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
|
||||
// do not
|
||||
List.of(
|
||||
|
||||
@@ -18,7 +18,10 @@ package org.photonvision.vision.target;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTagDetection;
|
||||
import edu.wpi.first.apriltag.AprilTagPoseEstimate;
|
||||
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 java.util.HashMap;
|
||||
import java.util.List;
|
||||
import org.opencv.core.CvType;
|
||||
@@ -28,6 +31,7 @@ import org.opencv.core.MatOfPoint2f;
|
||||
import org.opencv.core.Point;
|
||||
import org.opencv.core.RotatedRect;
|
||||
import org.photonvision.common.util.math.MathUtils;
|
||||
import org.photonvision.vision.aruco.ArucoDetectionResult;
|
||||
import org.photonvision.vision.frame.FrameStaticProperties;
|
||||
import org.photonvision.vision.opencv.*;
|
||||
|
||||
@@ -135,6 +139,59 @@ public class TrackedTarget implements Releasable {
|
||||
setCameraRelativeRvec(rvec);
|
||||
}
|
||||
|
||||
public TrackedTarget(ArucoDetectionResult result, 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);
|
||||
|
||||
double[] xCorners = result.getxCorners();
|
||||
double[] yCorners = result.getyCorners();
|
||||
|
||||
Point[] cornerPoints =
|
||||
new Point[] {
|
||||
new Point(xCorners[0], yCorners[0]),
|
||||
new Point(xCorners[1], yCorners[1]),
|
||||
new Point(xCorners[2], yCorners[2]),
|
||||
new Point(xCorners[3], yCorners[3])
|
||||
};
|
||||
m_targetCorners = List.of(cornerPoints);
|
||||
MatOfPoint contourMat = new MatOfPoint(cornerPoints);
|
||||
m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints);
|
||||
m_mainContour = new Contour(contourMat);
|
||||
m_area = m_mainContour.getArea() / params.imageArea * 100;
|
||||
m_fiducialId = result.getId();
|
||||
m_shape = null;
|
||||
|
||||
// TODO implement skew? or just yeet
|
||||
|
||||
var tvec = new Mat(3, 1, CvType.CV_64FC1);
|
||||
tvec.put(0, 0, result.getTvec());
|
||||
setCameraRelativeTvec(tvec);
|
||||
|
||||
var rvec = new Mat(3, 1, CvType.CV_64FC1);
|
||||
rvec.put(0, 0, result.getRvec());
|
||||
setCameraRelativeRvec(rvec);
|
||||
|
||||
{
|
||||
Translation3d translation =
|
||||
// new Translation3d(tVec.get(0, 0)[0], tVec.get(1, 0)[0], tVec.get(2, 0)[0]);
|
||||
new Translation3d(result.getTvec()[0], result.getTvec()[1], result.getTvec()[2]);
|
||||
var axisangle =
|
||||
VecBuilder.fill(result.getRvec()[0], result.getRvec()[1], result.getRvec()[2]);
|
||||
Rotation3d rotation = new Rotation3d(axisangle, axisangle.normF());
|
||||
Transform3d targetPose =
|
||||
MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation));
|
||||
|
||||
m_bestCameraToTarget3d = targetPose;
|
||||
}
|
||||
}
|
||||
|
||||
public void setFiducialId(int id) {
|
||||
m_fiducialId = id;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* 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.pipeline;
|
||||
|
||||
import java.io.IOException;
|
||||
import java.util.stream.Collectors;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.vision.camera.QuirkyCamera;
|
||||
import org.photonvision.vision.frame.provider.FileFrameProvider;
|
||||
import org.photonvision.vision.pipeline.result.CVPipelineResult;
|
||||
import org.photonvision.vision.target.TargetModel;
|
||||
import org.photonvision.vision.target.TrackedTarget;
|
||||
|
||||
public class ArucoPipelineTest {
|
||||
@BeforeEach
|
||||
public void Init() throws IOException {
|
||||
TestUtils.loadLibraries();
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testApriltagFacingCameraAruco() {
|
||||
var pipeline = new ArucoPipeline();
|
||||
|
||||
pipeline.getSettings().inputShouldShow = true;
|
||||
pipeline.getSettings().outputShouldDraw = true;
|
||||
pipeline.getSettings().solvePNPEnabled = true;
|
||||
pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
|
||||
pipeline.getSettings().cornerDetectionUseConvexHulls = true;
|
||||
pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
|
||||
|
||||
// pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
|
||||
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_16h5_1280, false),
|
||||
106,
|
||||
TestUtils.getCoeffs("laptop_1280.json", false));
|
||||
frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
|
||||
|
||||
CVPipelineResult pipelineResult;
|
||||
try {
|
||||
pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
|
||||
printTestResults(pipelineResult);
|
||||
} catch (RuntimeException e) {
|
||||
// For now, will throw coz rotation3d ctor
|
||||
return;
|
||||
}
|
||||
|
||||
// Draw on input
|
||||
var outputPipe = new OutputStreamPipeline();
|
||||
outputPipe.process(
|
||||
pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
|
||||
|
||||
TestUtils.showImage(
|
||||
pipelineResult.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
|
||||
}
|
||||
|
||||
private static void printTestResults(CVPipelineResult pipelineResult) {
|
||||
double fps = 1000 / pipelineResult.getLatencyMillis();
|
||||
System.out.println(
|
||||
"Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)");
|
||||
System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
|
||||
System.out.println(
|
||||
"Found targets at "
|
||||
+ pipelineResult.targets.stream()
|
||||
.map(TrackedTarget::getBestCameraToTarget3d)
|
||||
.collect(Collectors.toList()));
|
||||
}
|
||||
}
|
||||
33
test-resources/calibration/laptop_1280.json
Normal file
33
test-resources/calibration/laptop_1280.json
Normal file
@@ -0,0 +1,33 @@
|
||||
{
|
||||
"resolution": {
|
||||
"width": 1280.0,
|
||||
"height": 720.0
|
||||
},
|
||||
"cameraIntrinsics": {
|
||||
"rows": 3,
|
||||
"cols": 3,
|
||||
"type": 6,
|
||||
"data": [
|
||||
772.8811133362973, 0.0, 658.195907928895, 0.0, 774.614883980155,
|
||||
382.51346080446376, 0.0, 0.0, 1.0
|
||||
]
|
||||
},
|
||||
"cameraExtrinsics": {
|
||||
"rows": 1,
|
||||
"cols": 5,
|
||||
"type": 6,
|
||||
"data": [
|
||||
0.012379991516584503, -0.02480531511889502, -0.0019890797679669916,
|
||||
0.004880819847116537, 0.011416616592083246
|
||||
]
|
||||
},
|
||||
"perViewErrors": [
|
||||
0.3638546496153099, 0.320248621678274, 0.5961721612674044,
|
||||
0.36471868370313654, 0.8867304205602674, 0.33170668499682465,
|
||||
1.1510901610887656, 0.3135195406442026, 0.6278864159169598,
|
||||
0.8112949178024729, 0.3712810633477184, 0.9395284037997239,
|
||||
1.3962516818958053, 0.4955041432920438, 0.36239257683589704,
|
||||
0.8017916217152257, 0.3702170636543333
|
||||
],
|
||||
"standardDeviation": 0.3191763323035045
|
||||
}
|
||||
BIN
test-resources/testimages/apriltag/tag1_16h5_1280.png
Normal file
BIN
test-resources/testimages/apriltag/tag1_16h5_1280.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 462 KiB |
Reference in New Issue
Block a user