Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-04 03:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Run multitag on coprocessor (#816)

Browse Source
This commit is contained in:
Matt
2023-10-17 10:20:00 -04:00
committed by GitHub
parent ededc4f130
commit 47bd077bbb
72 changed files with 1708 additions and 1801 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -114,7 +114,6 @@ fabric.properties
**/.settings
**/.classpath
**/.project
**/settings
**/dependency-reduced-pom.xml
# photon-server/photon-vision.iml

2
photon-client/src/App.vue
View File

@@ -30,7 +30,7 @@ const websocket = new AutoReconnectingWebsocket(
useSettingsStore().updateMetricsFromWebsocket(data.metrics);
}
if (data.updatePipelineResult !== undefined) {
useStateStore().updatePipelineResultsFromWebsocket(data.updatePipelineResult);
useStateStore().updateBackendResultsFromWebsocket(data.updatePipelineResult);
}
if (data.mutatePipelineSettings !== undefined && data.cameraIndex !== undefined) {
useCameraSettingsStore().changePipelineSettingsInStore(data.mutatePipelineSettings, data.cameraIndex);

2
photon-client/src/components/app/photon-log-view.vue
View File

@@ -71,7 +71,7 @@ document.addEventListener("keydown", (e) => {
</v-btn>
</v-btn-toggle>
<v-card-text v-if="logs.length === 0" style="font-size: 18px; font-weight: 600">
There are no Logs to show
There are no logs to show
</v-card-text>
<v-virtual-scroll v-else :items="logs" item-height="50" height="600">
<template #default="{ item }">

6
photon-client/src/components/cameras/CamerasView.vue
View File

@@ -30,7 +30,7 @@ const driverMode = computed<boolean>({
});
const fpsTooLow = computed<boolean>(() => {
const currFPS = useStateStore().pipelineResults?.fps || 0;
const currFPS = useStateStore().currentPipelineResults?.fps || 0;
const targetFPS = useCameraSettingsStore().currentVideoFormat.fps;
const driverMode = useCameraSettingsStore().isDriverMode;
const gpuAccel = useSettingsStore().general.gpuAcceleration !== undefined;
@@ -58,8 +58,8 @@ const fpsTooLow = computed<boolean>(() => {
style="font-size: 1rem; padding: 0; margin: 0"
>
<span class="pr-1">
{{ Math.round(useStateStore().pipelineResults?.fps || 0) }}&nbsp;FPS &ndash;
{{ Math.min(Math.round(useStateStore().pipelineResults?.latency || 0), 9999) }} ms latency
{{ Math.round(useStateStore().currentPipelineResults?.fps || 0) }}&nbsp;FPS &ndash;
{{ Math.min(Math.round(useStateStore().currentPipelineResults?.latency || 0), 9999) }} ms latency
</span>
</v-chip>
</div>

6
photon-client/src/components/dashboard/CamerasCard.vue
View File

@@ -21,7 +21,7 @@ const driverMode = computed<boolean>({
});
const fpsTooLow = computed<boolean>(() => {
const currFPS = useStateStore().pipelineResults?.fps || 0;
const currFPS = useStateStore().currentPipelineResults?.fps || 0;
const targetFPS = useCameraSettingsStore().currentVideoFormat.fps;
const driverMode = useCameraSettingsStore().isDriverMode;
const gpuAccel = useSettingsStore().general.gpuAcceleration !== undefined;
@@ -46,7 +46,7 @@ const fpsTooLow = computed<boolean>(() => {
style="font-size: 1rem; padding: 0; margin: 0"
>
<span class="pr-1">
Processing @ {{ Math.round(useStateStore().pipelineResults?.fps || 0) }}&nbsp;FPS &ndash;
Processing @ {{ Math.round(useStateStore().currentPipelineResults?.fps || 0) }}&nbsp;FPS &ndash;
</span>
<span
v-if="
@@ -61,7 +61,7 @@ const fpsTooLow = computed<boolean>(() => {
stop viewing the raw stream for better performance
</span>
<span v-else>
{{ Math.min(Math.round(useStateStore().pipelineResults?.latency || 0), 9999) }} ms latency
{{ Math.min(Math.round(useStateStore().currentPipelineResults?.latency || 0), 9999) }} ms latency
</span>
</v-chip>
</div>

2
photon-client/src/components/dashboard/tabs/Map3DTab.vue
View File

@@ -4,7 +4,7 @@ import type { PhotonTarget } from "@/types/PhotonTrackingTypes";
import { useStateStore } from "@/stores/StateStore";
import Photon3dVisualizer from "@/components/app/photon-3d-visualizer.vue";
const trackedTargets = computed<PhotonTarget[]>(() => useStateStore().pipelineResults?.targets || []);
const trackedTargets = computed<PhotonTarget[]>(() => useStateStore().currentPipelineResults?.targets || []);
</script>
<template>

28
photon-client/src/components/dashboard/tabs/OutputTab.vue
View File

@@ -40,6 +40,8 @@ const offsetPoints = computed<MetricItem[]>(() => {
}
});
const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings;
const interactiveCols = computed(
() =>
(getCurrentInstance()?.proxy.$vuetify.breakpoint.mdAndDown || false) &&
@@ -75,13 +77,37 @@ const interactiveCols = computed(
<cv-switch
v-model="useCameraSettingsStore().currentPipelineSettings.outputShowMultipleTargets"
label="Show Multiple Targets"
tooltip="If enabled, up to five targets will be displayed and sent to user code, instead of just one"
tooltip="If enabled, up to five targets will be displayed and sent via PhotonLib, instead of just one"
:disabled="isTagPipeline"
:switch-cols="interactiveCols"
@input="
(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ outputShowMultipleTargets: value }, false)
"
/>
<cv-switch
v-if="
currentPipelineSettings.pipelineType === PipelineType.AprilTag &&
useCameraSettingsStore().isCurrentVideoFormatCalibrated
"
v-model="currentPipelineSettings.doMultiTarget"
label="Do Multi-Target Estimation"
tooltip="If enabled, all visible fiducial targets will be used to provide a single pose estimate from their combined model."
:switch-cols="interactiveCols"
:disabled="!isTagPipeline"
@input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ doMultiTarget: value }, false)"
/>
<cv-switch
v-if="
currentPipelineSettings.pipelineType === PipelineType.AprilTag &&
useCameraSettingsStore().isCurrentVideoFormatCalibrated
"
v-model="currentPipelineSettings.doSingleTargetAlways"
label="Always Do Single-Target Estimation"
tooltip="If disabled, visible fiducial targets used for multi-target estimation will not also be used for single-target estimation."
:switch-cols="interactiveCols"
:disabled="!isTagPipeline || !currentPipelineSettings.doMultiTarget"
@input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ doSingleTargetAlways: value }, false)"
/>
<v-divider />
<table
v-if="useCameraSettingsStore().currentPipelineSettings.offsetRobotOffsetMode !== RobotOffsetPointMode.None"

34
photon-client/src/components/dashboard/tabs/TargetsTab.vue
View File

@@ -8,11 +8,10 @@ import { useStateStore } from "@/stores/StateStore";
<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 height="100%" dense dark>
<v-simple-table fixed-header dense dark>
<template #default>
<thead style="font-size: 1.25rem">
<tr>
<th class="text-center">Target Count</th>
<th
v-if="
useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag ||
@@ -44,8 +43,7 @@ import { useStateStore } from "@/stores/StateStore";
</tr>
</thead>
<tbody>
<tr v-for="(target, index) in useStateStore().pipelineResults?.targets" :key="index">
<td>{{ index }}</td>
<tr v-for="(target, index) in useStateStore().currentPipelineResults?.targets" :key="index">
<td
v-if="
useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag ||
@@ -63,7 +61,7 @@ import { useStateStore } from "@/stores/StateStore";
<template v-else-if="useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled">
<td>{{ target.pose?.x.toFixed(2) }}&nbsp;m</td>
<td>{{ target.pose?.y.toFixed(2) }}&nbsp;m</td>
<td>{{ ((target.pose?.angle_z * 180.0) / Math.PI).toFixed(2) }}&deg;</td>
<td>{{ (((target.pose?.angle_z || 0) * 180.0) / Math.PI).toFixed(2) }}&deg;</td>
</template>
<template
v-if="
@@ -71,13 +69,37 @@ import { useStateStore } from "@/stores/StateStore";
useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled
"
>
<td>{{ target.ambiguity?.toFixed(2) }}%</td>
<td>{{ target.ambiguity >= 0 ? target.ambiguity?.toFixed(2) + "%" : "(In Multi-Target)" }}</td>
</template>
</tr>
</tbody>
</template>
</v-simple-table>
</v-row>
<v-row
v-if="
useCameraSettingsStore().currentPipelineSettings.pipelineType === PipelineType.AprilTag &&
useCameraSettingsStore().currentPipelineSettings.doMultiTarget
"
align="start"
class="pb-4 white--text"
>
<v-card-subtitle>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>
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }}</td>
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }}</td>
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z.toFixed(2) }}</td>
<td>{{ useStateStore().currentPipelineResults?.multitagResult?.fiducialIDsUsed }}</td>
</tbody>
</v-simple-table>
</v-row>
</div>
</template>

93
photon-client/src/components/settings/ApriltagControlCard.vue Normal file
View File

@@ -0,0 +1,93 @@
<script setup lang="ts">
import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
import { Euler, Quaternion as ThreeQuat } from "three";
import type { Quaternion } from "@/types/PhotonTrackingTypes";
const quatToEuler = (quat: Quaternion): Euler => {
const three_quat = new ThreeQuat(quat.X, quat.Y, quat.Z, quat.W);
return new Euler().setFromQuaternion(three_quat, "ZYX");
};
// Convert from radians to degrees.
const degrees = (radians: number): number => (radians * 180) / Math.PI;
</script>
<template>
<v-card dark class="mb-3 pr-6 pb-3" style="background-color: #006492">
<v-card-title>Apriltag Layout</v-card-title>
<div class="ml-5">
<p>Field width: {{ useSettingsStore().currentFieldLayout.field.width.toFixed(2) }} meters</p>
<p>Field length: {{ useSettingsStore().currentFieldLayout.field.length.toFixed(2) }} meters</p>
<!-- Simple table height must be set here and in the CSS for the fixed-header to work -->
<v-simple-table fixed-header height="100%" dense dark>
<template #default>
<thead style="font-size: 1.25rem">
<tr>
<th class="text-center">ID</th>
<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">θ<sub>x</sub> angle, &deg;</th>
<th class="text-center">θ<sub>y</sub> angle, &deg;</th>
<th class="text-center">θ<sub>z</sub> angle, &deg;</th>
</tr>
</thead>
<tbody>
<tr v-for="(tag, index) in useSettingsStore().currentFieldLayout.tags" :key="index">
<td>{{ tag.ID }}</td>
<td v-for="(val, idx) in Object.values(tag.pose.translation).slice(0, 3).map(degrees)" :key="idx">
{{ val.toFixed(2) }}
</td>
<td
v-for="(val, idx) in Object.values(quatToEuler(tag.pose.rotation.quaternion)).slice(0, 3).map(degrees)"
:key="idx"
>
{{ val.toFixed(2) }}
</td>
</tr>
</tbody>
</template>
</v-simple-table>
</div>
</v-card>
</template>
<style scoped lang="scss">
.v-data-table {
width: 100%;
height: 100%;
text-align: center;
background-color: #006492 !important;
th,
td {
background-color: #006492 !important;
font-size: 1rem !important;
}
td {
font-family: monospace !important;
}
tbody :hover td {
background-color: #005281 !important;
}
::-webkit-scrollbar {
width: 0;
height: 0.55em;
border-radius: 5px;
}
::-webkit-scrollbar-track {
-webkit-box-shadow: inset 0 0 6px rgba(0, 0, 0, 0.3);
border-radius: 10px;
}
::-webkit-scrollbar-thumb {
background-color: #ffd843;
border-radius: 10px;
}
}
</style>

14
photon-client/src/components/settings/DeviceControlCard.vue
View File

@@ -135,7 +135,8 @@ enum ImportType {
AllSettings,
HardwareConfig,
HardwareSettings,
NetworkConfig
NetworkConfig,
ApriltagFieldLayout
}
const showImportDialog = ref(false);
const importType = ref<ImportType | number>(-1);
@@ -157,6 +158,9 @@ const handleSettingsImport = () => {
case ImportType.NetworkConfig:
settingsEndpoint = "/networkConfig";
break;
case ImportType.ApriltagFieldLayout:
settingsEndpoint = "/aprilTagFieldLayout";
break;
default:
case ImportType.AllSettings:
settingsEndpoint = "";
@@ -249,7 +253,13 @@ const handleSettingsImport = () => {
v-model="importType"
label="Type"
tooltip="Select the type of settings file you are trying to upload"
:items="['All Settings', 'Hardware Config', 'Hardware Settings', 'Network Config']"
:items="[
'All Settings',
'Hardware Config',
'Hardware Settings',
'Network Config',
'Apriltag Layout'
]"
:select-cols="10"
style="width: 100%"
/>

4
photon-client/src/components/settings/MetricsCard.vue
View File

@@ -72,12 +72,12 @@ const fetchMetrics = () => {
if (error.request) {
useStateStore().showSnackbarMessage({
color: "error",
message: "Unable to fetch Metrics! The backend didn't respond."
message: "Unable to fetch metrics! The backend didn't respond."
});
} else {
useStateStore().showSnackbarMessage({
color: "error",
message: "An error occurred while trying to fetch Metrics."
message: "An error occurred while trying to fetch metrics."
});
}
})

20
photon-client/src/stores/StateStore.ts
View File

@@ -24,7 +24,7 @@ interface StateStore {
logMessages: LogMessage[];
currentCameraIndex: number;
pipelineResults?: PipelineResult;
backendResults: Record<string, PipelineResult>;
colorPickingMode: boolean;
@@ -58,7 +58,7 @@ export const useStateStore = defineStore("state", {
logMessages: [],
currentCameraIndex: 0,
pipelineResults: undefined,
backendResults: {},
colorPickingMode: false,
@@ -77,6 +77,11 @@ export const useStateStore = defineStore("state", {
}
};
},
getters: {
currentPipelineResults(): PipelineResult | undefined {
return this.backendResults[this.currentCameraIndex.toString()];
}
},
actions: {
setSidebarFolded(value: boolean) {
this.sidebarFolded = value;
@@ -95,12 +100,11 @@ export const useStateStore = defineStore("state", {
clients: data.clients
};
},
updatePipelineResultsFromWebsocket(data: WebsocketPipelineResultUpdate) {
for (const cameraIndex in data) {
if (parseInt(cameraIndex) === this.currentCameraIndex) {
this.pipelineResults = data[cameraIndex];
}
}
updateBackendResultsFromWebsocket(data: WebsocketPipelineResultUpdate) {
this.backendResults = {
...this.backendResults,
...data
};
},
updateCalibrationStateValuesFromWebsocket(data: WebsocketCalibrationData) {
this.calibrationData = {

4
photon-client/src/stores/settings/CameraSettingsStore.ts
View File

@@ -173,7 +173,9 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
settings: Partial<ActivePipelineSettings>,
cameraIndex: number = useStateStore().currentCameraIndex
) {
Object.assign(this.cameras[cameraIndex].pipelineSettings, settings);
Object.entries(settings).forEach(([k, v]) => {
this.cameras[cameraIndex].pipelineSettings[k] = v;
});
},
/**
* Change the nickname of the currently selected pipeline of the provided camera.

10
photon-client/src/stores/settings/GeneralSettingsStore.ts
View File

@@ -10,12 +10,14 @@ import { NetworkConnectionType } from "@/types/SettingTypes";
import { useStateStore } from "@/stores/StateStore";
import axios from "axios";
import type { WebsocketSettingsUpdate } from "@/types/WebsocketDataTypes";
import type { AprilTagFieldLayout } from "@/types/PhotonTrackingTypes";
interface GeneralSettingsStore {
general: GeneralSettings;
network: NetworkSettings;
lighting: LightingSettings;
metrics: MetricData;
currentFieldLayout: AprilTagFieldLayout;
}
export const useSettingsStore = defineStore("settings", {
@@ -55,6 +57,13 @@ export const useSettingsStore = defineStore("settings", {
cpuThr: undefined,
cpuUptime: undefined,
diskUtilPct: undefined
},
currentFieldLayout: {
field: {
length: 16.4592,
width: 8.2296
},
tags: []
}
}),
getters: {
@@ -91,6 +100,7 @@ export const useSettingsStore = defineStore("settings", {
};
this.lighting = data.lighting;
this.network = data.networkSettings;
this.currentFieldLayout = data.atfl;
},
saveGeneralSettings() {
const payload: Required<ConfigurableNetworkSettings> = {

41
photon-client/src/types/PhotonTrackingTypes.ts
View File

@@ -1,12 +1,39 @@
export interface Pose {
export interface Transform3d {
x: number;
y: number;
z: number;
angle_z: number;
qw: number;
qx: number;
qy: number;
qz: number;
angle_z: number;
}
export interface Quaternion {
X: number;
Y: number;
Z: number;
W: number;
}
export interface AprilTagFieldLayout {
field: {
length: number;
width: number;
};
tags: {
ID: number;
pose: {
translation: {
x: number;
y: number;
z: number;
};
rotation: {
quaternion: Quaternion;
};
};
}[];
}
export interface PhotonTarget {
@@ -19,11 +46,19 @@ export interface PhotonTarget {
// -1 if not set
fiducialId: number;
// undefined if 3d isn't enabled
pose?: Pose;
pose?: Transform3d;
}
export interface MultitagResult {
bestTransform: Transform3d;
bestReprojectionError: number;
fiducialIDsUsed: number[];
}
export interface PipelineResult {
fps: number;
latency: number;
targets: PhotonTarget[];
// undefined if multitag failed or non-tag pipeline
multitagResult?: MultitagResult;
}

7
photon-client/src/types/PipelineTypes.ts
View File

@@ -214,6 +214,8 @@ export interface AprilTagPipelineSettings extends PipelineSettings {
debug: boolean;
threads: number;
tagFamily: AprilTagFamily;
doMultiTarget: boolean;
doSingleTargetAlways: boolean;
}
export type ConfigurableAprilTagPipelineSettings = Partial<
Omit<AprilTagPipelineSettings, "pipelineType" | "hammingDist" | "debug">
@@ -236,7 +238,9 @@ export const DefaultAprilTagPipelineSettings: AprilTagPipelineSettings = {
refineEdges: true,
debug: false,
threads: 4,
tagFamily: AprilTagFamily.Family16h5
tagFamily: AprilTagFamily.Family16h5,
doMultiTarget: false,
doSingleTargetAlways: false
};
export interface ArucoPipelineSettings extends PipelineSettings {
@@ -270,6 +274,7 @@ export type ActivePipelineSettings =
| ColoredShapePipelineSettings
| AprilTagPipelineSettings
| ArucoPipelineSettings;
export type ActiveConfigurablePipelineSettings =
| ConfigurableReflectivePipelineSettings
| ConfigurableColoredShapePipelineSettings

32
photon-client/src/types/WebsocketDataTypes.ts
View File

@@ -1,5 +1,6 @@
import type { GeneralSettings, LightingSettings, LogLevel, MetricData, NetworkSettings } from "@/types/SettingTypes";
import type { ActivePipelineSettings } from "@/types/PipelineTypes";
import type { AprilTagFieldLayout, PipelineResult } from "@/types/PhotonTrackingTypes";
export interface WebsocketLogMessage {
logMessage: {
@@ -11,6 +12,7 @@ export interface WebsocketSettingsUpdate {
general: Required<GeneralSettings>;
lighting: Required<LightingSettings>;
networkSettings: NetworkSettings;
atfl: AprilTagFieldLayout;
}
export interface WebsocketNumberPair {
@@ -60,31 +62,9 @@ export interface WebsocketNTUpdate {
address?: string;
clients?: number;
}
export type WebsocketPipelineResultUpdate = Record<
number,
{
fps: number;
latency: number;
targets: {
yaw: number;
pitch: number;
skew: number;
area: number;
ambiguity: number;
fiducialId: number;
pose: {
angle_z: number;
qw: number;
qx: number;
x: number;
qy: number;
y: number;
qz: number;
z: number;
};
}[];
}
>;
// key is the index of the camera, value is that camera's result
export type WebsocketPipelineResultUpdate = Record<string, PipelineResult>;
export interface WebsocketCalibrationData {
patternWidth: number;
@@ -106,7 +86,7 @@ export interface IncomingWebsocketData {
updatePipelineResult?: WebsocketPipelineResultUpdate;
networkInfo?: {
possibleRios: string[];
deviceips: string[];
deviceIps: string[];
};
mutatePipelineSettings?: Partial<ActivePipelineSettings>;
cameraIndex?: number; // Sent when mutating pipeline settings to check against currently active

2
photon-client/src/views/GeneralSettingsView.vue
View File

@@ -4,6 +4,7 @@ import DeviceControlCard from "@/components/settings/DeviceControlCard.vue";
import NetworkingCard from "@/components/settings/NetworkingCard.vue";
import LightingControlCard from "@/components/settings/LEDControlCard.vue";
import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
import ApriltagControlCard from "@/components/settings/ApriltagControlCard.vue";
</script>
<template>
@@ -12,5 +13,6 @@ import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
<DeviceControlCard />
<NetworkingCard />
<LightingControlCard v-if="useSettingsStore().lighting.supported" />
<ApriltagControlCard />
</div>
</template>

4
photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java
View File

@@ -255,6 +255,10 @@ public class ConfigManager {
return m_provider.saveUploadedNetworkConfig(uploadPath);
}
public boolean saveUploadedAprilTagFieldLayout(Path uploadPath) {
return m_provider.saveUploadedAprilTagFieldLayout(uploadPath);
}
public void requestSave() {
logger.trace("Requesting save...");
saveRequestTimestamp = System.currentTimeMillis();

2
photon-core/src/main/java/org/photonvision/common/configuration/ConfigProvider.java
View File

@@ -39,4 +39,6 @@ public abstract class ConfigProvider {
public abstract boolean saveUploadedHardwareSettings(Path uploadPath);
public abstract boolean saveUploadedNetworkConfig(Path uploadPath);
public abstract boolean saveUploadedAprilTagFieldLayout(Path uploadPath);
}

46
photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java
View File

@@ -18,8 +18,11 @@
package org.photonvision.common.configuration;
import com.fasterxml.jackson.core.JsonProcessingException;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import java.io.File;
import java.io.IOException;
import java.io.UncheckedIOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.text.DateFormat;
@@ -45,12 +48,14 @@ class LegacyConfigProvider extends ConfigProvider {
public static final String HW_CFG_FNAME = "hardwareConfig.json";
public static final String HW_SET_FNAME = "hardwareSettings.json";
public static final String NET_SET_FNAME = "networkSettings.json";
public static final String ATFL_SET_FNAME = "apriltagFieldLayout.json";
private PhotonConfiguration config;
private final File hardwareConfigFile;
private final File hardwareSettingsFile;
private final File networkConfigFile;
private final File camerasFolder;
private final File apriltagFieldLayoutFile;
final File configDirectoryFile;
@@ -86,6 +91,8 @@ class LegacyConfigProvider extends ConfigProvider {
new File(Path.of(configDirectoryFile.toString(), HW_SET_FNAME).toUri());
this.networkConfigFile =
new File(Path.of(configDirectoryFile.toString(), NET_SET_FNAME).toUri());
this.apriltagFieldLayoutFile =
new File(Path.of(configDirectoryFile.toString(), ATFL_SET_FNAME).toUri());
this.camerasFolder = new File(Path.of(configDirectoryFile.toString(), "cameras").toUri());
settingsSaveThread = new Thread(this::saveAndWriteTask);
@@ -116,6 +123,7 @@ class LegacyConfigProvider extends ConfigProvider {
HardwareConfig hardwareConfig;
HardwareSettings hardwareSettings;
NetworkConfig networkConfig;
AprilTagFieldLayout atfl = null;
if (hardwareConfigFile.exists()) {
try {
@@ -175,11 +183,38 @@ class LegacyConfigProvider extends ConfigProvider {
}
}
if (apriltagFieldLayoutFile.exists()) {
try {
atfl =
JacksonUtils.deserialize(apriltagFieldLayoutFile.toPath(), AprilTagFieldLayout.class);
if (atfl == null) {
logger.error("Could not deserialize apriltag field layout! (still null)");
}
} catch (IOException e) {
logger.error("Could not deserialize apriltag field layout!", e);
atfl = null; // not required, nice to be explicit
}
}
if (atfl == null) {
logger.info("Loading default apriltags for 2023 field...");
try {
atfl = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
} catch (UncheckedIOException e) {
logger.error("Error loading WPILib field", e);
atfl = null;
}
if (atfl == null) {
// what do we even do here lmao -- wpilib built-in should always work
logger.error("Field layout is *still* null??????");
atfl = new AprilTagFieldLayout(List.of(), 1, 1);
}
}
HashMap<String, CameraConfiguration> cameraConfigurations = loadCameraConfigs();
this.config =
new PhotonConfiguration(
hardwareConfig, hardwareSettings, networkConfig, cameraConfigurations);
hardwareConfig, hardwareSettings, networkConfig, atfl, cameraConfigurations);
}
@Override
@@ -400,6 +435,10 @@ class LegacyConfigProvider extends ConfigProvider {
return this.networkConfigFile.toPath();
}
public Path getAprilTagFieldLayoutFile() {
return this.apriltagFieldLayoutFile.toPath();
}
@Override
public boolean saveUploadedHardwareConfig(Path uploadPath) {
return FileUtils.replaceFile(uploadPath, this.getHardwareConfigFile());
@@ -415,6 +454,11 @@ class LegacyConfigProvider extends ConfigProvider {
return FileUtils.replaceFile(uploadPath, this.getNetworkConfigFile());
}
@Override
public boolean saveUploadedAprilTagFieldLayout(Path uploadPath) {
return FileUtils.replaceFile(uploadPath, this.getAprilTagFieldLayoutFile());
}
public void requestSave() {
logger.trace("Requesting save...");
saveRequestTimestamp = System.currentTimeMillis();

37
photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
View File

@@ -17,6 +17,7 @@
package org.photonvision.common.configuration;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import java.util.Collection;
import java.util.HashMap;
import java.util.List;
@@ -31,33 +32,40 @@ import org.photonvision.vision.processes.VisionModule;
import org.photonvision.vision.processes.VisionModuleManager;
import org.photonvision.vision.processes.VisionSource;
// TODO rename this class
public class PhotonConfiguration {
private final HardwareConfig hardwareConfig;
private final HardwareSettings hardwareSettings;
private NetworkConfig networkConfig;
private final HashMap<String, CameraConfiguration> cameraConfigurations;
private AprilTagFieldLayout atfl;
private HashMap<String, CameraConfiguration> cameraConfigurations;
public PhotonConfiguration(
HardwareConfig hardwareConfig,
HardwareSettings hardwareSettings,
NetworkConfig networkConfig) {
this(hardwareConfig, hardwareSettings, networkConfig, new HashMap<>());
NetworkConfig networkConfig,
AprilTagFieldLayout atfl) {
this(hardwareConfig, hardwareSettings, networkConfig, atfl, new HashMap<>());
}
public PhotonConfiguration(
HardwareConfig hardwareConfig,
HardwareSettings hardwareSettings,
NetworkConfig networkConfig,
AprilTagFieldLayout atfl,
HashMap<String, CameraConfiguration> cameraConfigurations) {
this.hardwareConfig = hardwareConfig;
this.hardwareSettings = hardwareSettings;
this.networkConfig = networkConfig;
this.cameraConfigurations = cameraConfigurations;
this.atfl = atfl;
}
public PhotonConfiguration() {
this(new HardwareConfig(), new HardwareSettings(), new NetworkConfig());
this(
new HardwareConfig(),
new HardwareSettings(),
new NetworkConfig(),
new AprilTagFieldLayout(List.of(), 0, 0));
}
public HardwareConfig getHardwareConfig() {
@@ -72,6 +80,14 @@ public class PhotonConfiguration {
return hardwareSettings;
}
public AprilTagFieldLayout getApriltagFieldLayout() {
return atfl;
}
public void setApriltagFieldLayout(AprilTagFieldLayout atfl) {
this.atfl = atfl;
}
public void setNetworkConfig(NetworkConfig networkConfig) {
this.networkConfig = networkConfig;
}
@@ -115,7 +131,7 @@ public class PhotonConfiguration {
lightingConfig.brightness = hardwareSettings.ledBrightnessPercentage;
lightingConfig.supported = !hardwareConfig.ledPins.isEmpty();
settingsSubmap.put("lighting", SerializationUtils.objectToHashMap(lightingConfig));
// General Settings
var generalSubmap = new HashMap<String, Object>();
generalSubmap.put("version", PhotonVersion.versionString);
generalSubmap.put(
@@ -126,8 +142,17 @@ public class PhotonConfiguration {
generalSubmap.put("hardwareModel", hardwareConfig.deviceName);
generalSubmap.put("hardwarePlatform", Platform.getPlatformName());
settingsSubmap.put("general", generalSubmap);
// AprilTagFieldLayout
settingsSubmap.put("atfl", this.atfl);
map.put(
"cameraSettings",
VisionModuleManager.getInstance().getModules().stream()
.map(VisionModule::toUICameraConfig)
.map(SerializationUtils::objectToHashMap)
.collect(Collectors.toList()));
map.put("settings", settingsSubmap);
return map;
}

32
photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java
View File

@@ -17,8 +17,11 @@
package org.photonvision.common.configuration;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import java.io.File;
import java.io.IOException;
import java.io.UncheckedIOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.sql.*;
@@ -53,6 +56,7 @@ public class SqlConfigProvider extends ConfigProvider {
static final String NETWORK_CONFIG = "networkConfig";
static final String HARDWARE_CONFIG = "hardwareConfig";
static final String HARDWARE_SETTINGS = "hardwareSettings";
static final String ATFL_CONFIG_FILE = "apriltagFieldLayout";
}
private static final String dbName = "photon.sqlite";
@@ -201,6 +205,7 @@ public class SqlConfigProvider extends ConfigProvider {
HardwareConfig hardwareConfig;
HardwareSettings hardwareSettings;
NetworkConfig networkConfig;
AprilTagFieldLayout atfl;
try {
hardwareConfig =
@@ -229,6 +234,25 @@ public class SqlConfigProvider extends ConfigProvider {
networkConfig = new NetworkConfig();
}
try {
atfl =
JacksonUtils.deserialize(
getOneConfigFile(conn, TableKeys.ATFL_CONFIG_FILE), AprilTagFieldLayout.class);
} catch (IOException e) {
logger.error("Could not deserialize apriltag layout! Loading defaults");
try {
atfl = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
} catch (UncheckedIOException e2) {
logger.error("Error loading WPILib field", e);
atfl = null;
}
if (atfl == null) {
// what do we even do here lmao -- wpilib should always work
logger.error("Field layout is *still* null??????");
atfl = new AprilTagFieldLayout(List.of(), 1, 1);
}
}
var cams = loadCameraConfigs(conn);
try {
@@ -237,7 +261,8 @@ public class SqlConfigProvider extends ConfigProvider {
logger.error("SQL Err closing connection while loading: ", e);
}
this.config = new PhotonConfiguration(hardwareConfig, hardwareSettings, networkConfig, cams);
this.config =
new PhotonConfiguration(hardwareConfig, hardwareSettings, networkConfig, atfl, cams);
}
}
@@ -416,6 +441,11 @@ public class SqlConfigProvider extends ConfigProvider {
return saveOneFile(TableKeys.NETWORK_CONFIG, uploadPath);
}
@Override
public boolean saveUploadedAprilTagFieldLayout(Path uploadPath) {
return saveOneFile(TableKeys.ATFL_CONFIG_FILE, uploadPath);
}
private HashMap<String, CameraConfiguration> loadCameraConfigs(Connection conn) {
HashMap<String, CameraConfiguration> loadedConfigurations = new HashMap<>();

44
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -19,20 +19,15 @@ package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEvent;
import java.util.ArrayList;
import java.util.List;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
import java.util.function.Supplier;
import org.opencv.core.Point;
import org.photonvision.common.dataflow.CVPipelineResultConsumer;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.TrackedTarget;
@@ -136,7 +131,9 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
public void accept(CVPipelineResult result) {
var simplified =
new PhotonPipelineResult(
result.getLatencyMillis(), simpleFromTrackedTargets(result.targets));
result.getLatencyMillis(),
TrackedTarget.simpleFromTrackedTargets(result.targets),
result.multiTagResult);
Packet packet = new Packet(simplified.getPacketSize());
simplified.populatePacket(packet);
@@ -197,39 +194,4 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
// TODO...nt4... is this needed?
rootTable.getInstance().flush();
}
public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
var ret = new ArrayList<PhotonTrackedTarget>();
for (var t : targets) {
var minAreaRectCorners = new ArrayList<TargetCorner>();
var detectedCorners = new ArrayList<TargetCorner>();
{
var points = new Point[4];
t.getMinAreaRect().points(points);
for (int i = 0; i < 4; i++) {
minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y));
}
}
{
var points = t.getTargetCorners();
for (Point point : points) {
detectedCorners.add(new TargetCorner(point.x, point.y));
}
}
ret.add(
new PhotonTrackedTarget(
t.getYaw(),
t.getPitch(),
t.getArea(),
t.getSkew(),
t.getFiducialId(),
t.getBestCameraToTarget3d(),
t.getAltCameraToTarget3d(),
t.getPoseAmbiguity(),
minAreaRectCorners,
detectedCorners));
}
return ret;
}
}

83
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
View File

@@ -17,9 +17,14 @@
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEvent;
import edu.wpi.first.networktables.NetworkTableEvent.Kind;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringSubscriber;
import java.io.IOException;
import java.util.EnumSet;
import java.util.HashMap;
import java.util.function.Consumer;
import org.photonvision.PhotonVersion;
@@ -32,17 +37,33 @@ import org.photonvision.common.logging.Logger;
import org.photonvision.common.scripting.ScriptEventType;
import org.photonvision.common.scripting.ScriptManager;
import org.photonvision.common.util.TimedTaskManager;
import org.photonvision.common.util.file.JacksonUtils;
public class NetworkTablesManager {
private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
private final String kRootTableName = "/photonvision";
private final String kFieldLayoutName = "apriltag_field_layout";
public final NetworkTable kRootTable = ntInstance.getTable(kRootTableName);
private boolean isRetryingConnection = false;
private final NTLogger m_ntLogger = new NTLogger();
private boolean m_isRetryingConnection = false;
private StringSubscriber m_fieldLayoutSubscriber =
kRootTable.getStringTopic(kFieldLayoutName).subscribe("");
private NetworkTablesManager() {
ntInstance.addLogger(0, 255, new NTLogger()); // to hide error messages
ntInstance.addLogger(255, 255, (event) -> {}); // to hide error messages
ntInstance.addConnectionListener(true, m_ntLogger); // to hide error messages
ntInstance.addListener(
m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged);
TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
// Get the UI state in sync with the backend. NT should fire a callback when it first connects
// to the robot
broadcastConnectedStatus();
}
private static NetworkTablesManager INSTANCE;
@@ -56,24 +77,33 @@ public class NetworkTablesManager {
private static class NTLogger implements Consumer<NetworkTableEvent> {
private boolean hasReportedConnectionFailure = false;
private long lastConnectMessageMillis = 0;
@Override
public void accept(NetworkTableEvent event) {
if (!hasReportedConnectionFailure && event.logMessage.message.contains("timed out")) {
logger.error("NT Connection has failed! Will retry in background.");
var isConnEvent = event.is(Kind.kConnected);
var isDisconnEvent = event.is(Kind.kDisconnected);
if (!hasReportedConnectionFailure && isDisconnEvent) {
var msg =
String.format(
"NT lost connection to %s:%d! (NT version %d). Will retry in background.",
event.connInfo.remote_ip,
event.connInfo.remote_port,
event.connInfo.protocol_version);
logger.error(msg);
hasReportedConnectionFailure = true;
getInstance().broadcastConnectedStatus();
} else if (event.logMessage.message.contains("connected")
&& System.currentTimeMillis() - lastConnectMessageMillis > 125) {
String connectionDescription = "(unknown)";
var connections = getInstance().ntInstance.getConnections();
if (connections.length > 0) {
connectionDescription = connections[0].remote_ip + ":" + connections[0].remote_port;
}
logger.info("NT Connected to " + connectionDescription + "!");
} else if (isConnEvent && event.connInfo != null) {
var msg =
String.format(
"NT connected to %s:%d! (NT version %d)",
event.connInfo.remote_ip,
event.connInfo.remote_port,
event.connInfo.protocol_version);
logger.info(msg);
hasReportedConnectionFailure = false;
lastConnectMessageMillis = System.currentTimeMillis();
ScriptManager.queueEvent(ScriptEventType.kNTConnected);
getInstance().broadcastVersion();
getInstance().broadcastConnectedStatus();
@@ -81,6 +111,23 @@ public class NetworkTablesManager {
}
}
private void onFieldLayoutChanged(NetworkTableEvent event) {
var atfl_json = event.valueData.value.getString();
try {
System.out.println("Got new field layout!");
var atfl = JacksonUtils.deserialize(atfl_json, AprilTagFieldLayout.class);
ConfigManager.getInstance().getConfig().setApriltagFieldLayout(atfl);
ConfigManager.getInstance().requestSave();
DataChangeService.getInstance()
.publishEvent(
new OutgoingUIEvent<>(
"fullsettings", ConfigManager.getInstance().getConfig().toHashMap()));
} catch (IOException e) {
logger.error("Error deserializing atfl!");
logger.error(atfl_json);
}
}
public void broadcastConnectedStatus() {
TimedTaskManager.getInstance().addOneShotTask(this::broadcastConnectedStatusImpl, 1000L);
}
@@ -122,10 +169,10 @@ public class NetworkTablesManager {
ntInstance.startClient4("photonvision");
try {
int t = Integer.parseInt(ntServerAddress);
if (!isRetryingConnection) logger.info("Starting NT Client, server team is " + t);
if (!m_isRetryingConnection) logger.info("Starting NT Client, server team is " + t);
ntInstance.setServerTeam(t);
} catch (NumberFormatException e) {
if (!isRetryingConnection)
if (!m_isRetryingConnection)
logger.info("Starting NT Client, server IP is \"" + ntServerAddress + "\"");
ntInstance.setServer(ntServerAddress);
}
@@ -151,8 +198,8 @@ public class NetworkTablesManager {
setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig());
}
if (!ntInstance.isConnected() && !isRetryingConnection) {
isRetryingConnection = true;
if (!ntInstance.isConnected() && !m_isRetryingConnection) {
m_isRetryingConnection = true;
logger.error(
"[NetworkTablesManager] Could not connect to the robot! Will retry in the background...");
}

28
photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
View File

@@ -24,6 +24,7 @@ import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
public class UIDataPublisher implements CVPipelineResultConsumer {
@@ -40,23 +41,30 @@ public class UIDataPublisher implements CVPipelineResultConsumer {
public void accept(CVPipelineResult result) {
long now = System.currentTimeMillis();
var dataMap = new HashMap<String, Object>();
dataMap.put("latency", result.getLatencyMillis());
// only update the UI at 15hz
if (lastUIResultUpdateTime + 1000.0 / 10.0 > now) return;
var uiMap = new HashMap<Integer, HashMap<String, Object>>();
var dataMap = new HashMap<String, Object>();
dataMap.put("fps", result.fps);
var targets = result.targets;
var uiTargets = new ArrayList<HashMap<String, Object>>();
for (var t : targets) {
dataMap.put("latency", result.getLatencyMillis());
var uiTargets = new ArrayList<HashMap<String, Object>>(result.targets.size());
for (var t : result.targets) {
uiTargets.add(t.toHashMap());
}
dataMap.put("targets", uiTargets);
// Only send Multitag Results if they are present, similar to 3d pose
if (result.multiTagResult.estimatedPose.isPresent) {
var multitagData = new HashMap<String, Object>();
multitagData.put(
"bestTransform",
SerializationUtils.transformToHashMap(result.multiTagResult.estimatedPose.best));
multitagData.put("bestReprojectionError", result.multiTagResult.estimatedPose.bestReprojErr);
multitagData.put("fiducialIDsUsed", result.multiTagResult.fiducialIDsUsed);
dataMap.put("multitagResult", multitagData);
}
var uiMap = new HashMap<Integer, HashMap<String, Object>>();
uiMap.put(index, dataMap);
DataChangeService.getInstance()

2
photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java
View File

@@ -138,6 +138,8 @@ public class HardwareManager {
private void onJvmExit() {
logger.info("Shutting down LEDs...");
if (visionLED != null) visionLED.setState(false);
logger.info("Force-flushing settings...");
ConfigManager.getInstance().saveToDisk();
}

2
photon-core/src/main/java/org/photonvision/common/networking/RoborioFinder.java
View File

@@ -45,7 +45,7 @@ public class RoborioFinder {
// Separate from the above so we don't hold stuff up
System.setProperty("java.net.preferIPv4Stack", "true");
subMap.put(
"deviceips",
"deviceIps",
Arrays.stream(CameraServerJNI.getNetworkInterfaces())
.filter(it -> !it.equals("0.0.0.0"))
.toArray());

15
photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java
View File

@@ -17,6 +17,7 @@
package org.photonvision.common.util;
import edu.wpi.first.math.geometry.Transform3d;
import java.util.HashMap;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
@@ -43,4 +44,18 @@ public final class SerializationUtils {
}
return ret;
}
public static HashMap<String, Object> transformToHashMap(Transform3d transform) {
var ret = new HashMap<String, Object>();
ret.put("x", transform.getTranslation().getX());
ret.put("y", transform.getTranslation().getY());
ret.put("z", transform.getTranslation().getZ());
ret.put("qw", transform.getRotation().getQuaternion().getW());
ret.put("qx", transform.getRotation().getQuaternion().getX());
ret.put("qy", transform.getRotation().getQuaternion().getY());
ret.put("qz", transform.getRotation().getQuaternion().getZ());
ret.put("angle_z", transform.getRotation().getZ());
return ret;
}
}

16
photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java
View File

@@ -19,7 +19,10 @@ package org.photonvision.vision.calibration;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import java.util.Arrays;
import org.ejml.simple.SimpleMatrix;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
@@ -32,7 +35,10 @@ public class JsonMat implements Releasable {
public final int type;
public final double[] data;
@JsonIgnore private Mat wrappedMat;
// Cached matrices to avoid object recreation
@JsonIgnore private Mat wrappedMat = null;
@JsonIgnore private Matrix wpilibMat = null;
private MatOfDouble wrappedMatOfDouble;
public JsonMat(int rows, int cols, double[] data) {
@@ -103,6 +109,14 @@ public class JsonMat implements Releasable {
return this.wrappedMatOfDouble;
}
@JsonIgnore
public <R extends Num, C extends Num> Matrix<R, C> getAsWpilibMat() {
if (wpilibMat == null) {
wpilibMat = new Matrix<R, C>(new SimpleMatrix(rows, cols, true, data));
}
return (Matrix<R, C>) wpilibMat;
}
@Override
public void release() {
getAsMat().release();

6
photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java
View File

@@ -27,8 +27,6 @@ public class AprilTagDetectionPipe
extends CVPipe<CVMat, List<AprilTagDetection>, AprilTagDetectionPipeParams> {
private final AprilTagDetector m_detector = new AprilTagDetector();
boolean useNativePoseEst;
public AprilTagDetectionPipe() {
super();
@@ -62,8 +60,4 @@ public class AprilTagDetectionPipe
super.setParams(newParams);
}
public void setNativePoseEstimationEnabled(boolean enabled) {
this.useNativePoseEst = enabled;
}
}

6
photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java
View File

@@ -35,8 +35,6 @@ public class AprilTagPoseEstimatorPipe
private final AprilTagPoseEstimator m_poseEstimator =
new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0, 0, 0, 0, 0));
boolean useNativePoseEst;
public AprilTagPoseEstimatorPipe() {
super();
}
@@ -94,10 +92,6 @@ public class AprilTagPoseEstimatorPipe
super.setParams(newParams);
}
public void setNativePoseEstimationEnabled(boolean enabled) {
this.useNativePoseEst = enabled;
}
public static class AprilTagPoseEstimatorPipeParams {
final AprilTagPoseEstimator.Config config;
final CameraCalibrationCoefficients calibration;

88
photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java Normal file
View File

@@ -0,0 +1,88 @@
/*
* 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.apriltag.AprilTagFieldLayout;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.pipe.CVPipe;
import org.photonvision.vision.target.TrackedTarget;
/** Estimate the camera pose given multiple Apriltag observations */
public class MultiTargetPNPPipe
extends CVPipe<
List<TrackedTarget>, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule);
private boolean hasWarned = false;
@Override
protected MultiTargetPNPResults process(List<TrackedTarget> targetList) {
if (params.cameraCoefficients == null
|| params.cameraCoefficients.getCameraIntrinsicsMat() == null
|| params.cameraCoefficients.getDistCoeffsMat() == null) {
if (!hasWarned) {
logger.warn(
"Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution...");
hasWarned = true;
}
return new MultiTargetPNPResults();
}
return calculateCameraInField(targetList);
}
private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetList) {
// Find tag IDs that exist in the tag layout
var tagIDsUsed = new ArrayList<Integer>();
for (var target : targetList) {
int id = target.getFiducialId();
if (params.atfl.getTagPose(id).isPresent()) tagIDsUsed.add(id);
}
// Only run with multiple targets
if (tagIDsUsed.size() < 2) {
return new MultiTargetPNPResults();
}
var estimatedPose =
VisionEstimation.estimateCamPosePNP(
params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(),
params.cameraCoefficients.distCoeffs.getAsWpilibMat(),
TrackedTarget.simpleFromTrackedTargets(targetList),
params.atfl);
return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);
}
public static class MultiTargetPNPPipeParams {
private final CameraCalibrationCoefficients cameraCoefficients;
private final AprilTagFieldLayout atfl;
public MultiTargetPNPPipeParams(
CameraCalibrationCoefficients cameraCoefficients, AprilTagFieldLayout atfl) {
this.cameraCoefficients = cameraCoefficients;
this.atfl = atfl;
}
}
}

139
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View File

@@ -21,11 +21,16 @@ import edu.wpi.first.apriltag.AprilTagDetection;
import edu.wpi.first.apriltag.AprilTagDetector;
import edu.wpi.first.apriltag.AprilTagPoseEstimate;
import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config;
import edu.wpi.first.math.geometry.CoordinateSystem;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
@@ -34,13 +39,17 @@ import org.photonvision.vision.pipe.impl.AprilTagDetectionPipeParams;
import org.photonvision.vision.pipe.impl.AprilTagPoseEstimatorPipe;
import org.photonvision.vision.pipe.impl.AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams;
import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
import org.photonvision.vision.pipe.impl.MultiTargetPNPPipe;
import org.photonvision.vision.pipe.impl.MultiTargetPNPPipe.MultiTargetPNPPipeParams;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.TrackedTarget;
import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipelineSettings> {
private final AprilTagDetectionPipe aprilTagDetectionPipe = new AprilTagDetectionPipe();
private final AprilTagPoseEstimatorPipe poseEstimatorPipe = new AprilTagPoseEstimatorPipe();
private final AprilTagPoseEstimatorPipe singleTagPoseEstimatorPipe =
new AprilTagPoseEstimatorPipe();
private final MultiTargetPNPPipe multiTagPNPPipe = new MultiTargetPNPPipe();
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.GREYSCALE;
@@ -91,17 +100,22 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
if (frameStaticProperties.cameraCalibration != null) {
var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
if (cameraMatrix != null) {
if (cameraMatrix != null && cameraMatrix.rows() > 0) {
var cx = cameraMatrix.get(0, 2)[0];
var cy = cameraMatrix.get(1, 2)[0];
var fx = cameraMatrix.get(0, 0)[0];
var fy = cameraMatrix.get(1, 1)[0];
poseEstimatorPipe.setParams(
singleTagPoseEstimatorPipe.setParams(
new AprilTagPoseEstimatorPipeParams(
new Config(tagWidth, fx, fy, cx, cy),
frameStaticProperties.cameraCalibration,
settings.numIterations));
// TODO global state ew
var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
multiTagPNPPipe.setParams(
new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl));
}
}
}
@@ -110,58 +124,119 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings settings) {
long sumPipeNanosElapsed = 0L;
List<TrackedTarget> targetList;
// Use the solvePNP Enabled flag to enable native pose estimation
aprilTagDetectionPipe.setNativePoseEstimationEnabled(settings.solvePNPEnabled);
if (frame.type != FrameThresholdType.GREYSCALE) {
// TODO so all cameras should give us ADAPTIVE_THRESH -- how should we handle if not?
return new CVPipelineResult(0, 0, List.of());
// TODO so all cameras should give us GREYSCALE -- how should we handle if not?
// Right now, we just return nothing
return new CVPipelineResult(0, 0, List.of(), frame);
}
CVPipeResult<List<AprilTagDetection>> tagDetectionPipeResult;
tagDetectionPipeResult = aprilTagDetectionPipe.run(frame.processedImage);
sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
targetList = new ArrayList<>();
for (AprilTagDetection detection : tagDetectionPipeResult.output) {
List<AprilTagDetection> detections = tagDetectionPipeResult.output;
List<AprilTagDetection> usedDetections = new ArrayList<>();
List<TrackedTarget> targetList = new ArrayList<>();
// Filter out detections based on pipeline settings
for (AprilTagDetection detection : detections) {
// TODO this should be in a pipe, not in the top level here (Matt)
if (detection.getDecisionMargin() < settings.decisionMargin) continue;
if (detection.getHamming() > settings.hammingDist) continue;
AprilTagPoseEstimate tagPoseEstimate = null;
if (settings.solvePNPEnabled) {
var poseResult = poseEstimatorPipe.run(detection);
sumPipeNanosElapsed += poseResult.nanosElapsed;
tagPoseEstimate = poseResult.output;
}
usedDetections.add(detection);
// populate the target list
// Challenge here is that TrackedTarget functions with OpenCV Contour
// Populate target list for multitag
// (TODO: Address circular dependencies. Multitag only requires corners and IDs, this should
// not be necessary.)
TrackedTarget target =
new TrackedTarget(
detection,
tagPoseEstimate,
null,
new TargetCalculationParameters(
false, null, null, null, null, frameStaticProperties));
var correctedBestPose =
MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d());
var correctedAltPose =
MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d());
target.setBestCameraToTarget3d(
new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
target.setAltCameraToTarget3d(
new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
targetList.add(target);
}
// Do multi-tag pose estimation
MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
if (settings.solvePNPEnabled && settings.doMultiTarget) {
var multiTagOutput = multiTagPNPPipe.run(targetList);
sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
multiTagResult = multiTagOutput.output;
}
// Do single-tag pose estimation
if (settings.solvePNPEnabled) {
// Clear target list that was used for multitag so we can add target transforms
targetList.clear();
// TODO global state again ew
var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
for (AprilTagDetection detection : usedDetections) {
AprilTagPoseEstimate tagPoseEstimate = null;
// Do single-tag estimation when "always enabled" or if a tag was not used for multitag
if (settings.doSingleTargetAlways
|| !multiTagResult.fiducialIDsUsed.contains(Integer.valueOf(detection.getId()))) {
var poseResult = singleTagPoseEstimatorPipe.run(detection);
sumPipeNanosElapsed += poseResult.nanosElapsed;
tagPoseEstimate = poseResult.output;
}
// If single-tag estimation was not done, this is a multi-target tag from the layout
if (tagPoseEstimate == null) {
// compute this tag's camera-to-tag transform using the multitag result
var tagPose = atfl.getTagPose(detection.getId());
if (tagPose.isPresent()) {
var camToTag =
new Transform3d(
new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
// match expected AprilTag coordinate system
// TODO cleanup coordinate systems in wpilib 2024
var apriltagTrl =
CoordinateSystem.convert(
camToTag.getTranslation(), CoordinateSystem.NWU(), CoordinateSystem.EDN());
var apriltagRot =
CoordinateSystem.convert(
new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU())
.plus(
CoordinateSystem.convert(
camToTag.getRotation(),
CoordinateSystem.NWU(),
CoordinateSystem.EDN()));
apriltagRot = new Rotation3d(0, Math.PI, 0).plus(apriltagRot);
camToTag = new Transform3d(apriltagTrl, apriltagRot);
tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0);
}
}
// populate the target list
// Challenge here is that TrackedTarget functions with OpenCV Contour
TrackedTarget target =
new TrackedTarget(
detection,
tagPoseEstimate,
new TargetCalculationParameters(
false, null, null, null, null, frameStaticProperties));
var correctedBestPose =
MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d());
var correctedAltPose =
MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d());
target.setBestCameraToTarget3d(
new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
target.setAltCameraToTarget3d(
new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
targetList.add(target);
}
}
var fpsResult = calculateFPSPipe.run(null);
var fps = fpsResult.output;
return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, frame);
return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
}
}

51
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
View File

@@ -18,7 +18,6 @@
package org.photonvision.vision.pipeline;
import com.fasterxml.jackson.annotation.JsonTypeName;
import java.util.Objects;
import org.photonvision.vision.apriltag.AprilTagFamily;
import org.photonvision.vision.target.TargetModel;
@@ -33,6 +32,8 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
public int numIterations = 40;
public int hammingDist = 0;
public int decisionMargin = 35;
public boolean doMultiTarget = true;
public boolean doSingleTargetAlways = false;
// 3d settings
@@ -47,16 +48,42 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
if (!super.equals(o)) return false;
AprilTagPipelineSettings that = (AprilTagPipelineSettings) o;
return Objects.equals(tagFamily, that.tagFamily)
&& Double.compare(decimate, that.decimate) == 0
&& Double.compare(blur, that.blur) == 0
&& threads == that.threads
&& debug == that.debug
&& refineEdges == that.refineEdges;
public int hashCode() {
final int prime = 31;
int result = super.hashCode();
result = prime * result + ((tagFamily == null) ? 0 : tagFamily.hashCode());
result = prime * result + decimate;
long temp;
temp = Double.doubleToLongBits(blur);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + threads;
result = prime * result + (debug ? 1231 : 1237);
result = prime * result + (refineEdges ? 1231 : 1237);
result = prime * result + numIterations;
result = prime * result + hammingDist;
result = prime * result + decisionMargin;
result = prime * result + (doMultiTarget ? 1231 : 1237);
result = prime * result + (doSingleTargetAlways ? 1231 : 1237);
return result;
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (!super.equals(obj)) return false;
if (getClass() != obj.getClass()) return false;
AprilTagPipelineSettings other = (AprilTagPipelineSettings) obj;
if (tagFamily != other.tagFamily) return false;
if (decimate != other.decimate) return false;
if (Double.doubleToLongBits(blur) != Double.doubleToLongBits(other.blur)) return false;
if (threads != other.threads) return false;
if (debug != other.debug) return false;
if (refineEdges != other.refineEdges) return false;
if (numIterations != other.numIterations) return false;
if (hammingDist != other.hammingDist) return false;
if (decisionMargin != other.decisionMargin) return false;
if (doMultiTarget != other.doMultiTarget) return false;
if (doSingleTargetAlways != other.doSingleTargetAlways) return false;
return true;
}
}

2
photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
View File

@@ -34,6 +34,7 @@ import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.common.util.file.FileUtils;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
@@ -146,6 +147,7 @@ public class Calibrate3dPipeline
sumPipeNanosElapsed,
fps, // Unused but here in case
Collections.emptyList(),
new MultiTargetPNPResults(),
new Frame(
new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties));
}

20
photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
View File

@@ -20,6 +20,7 @@ package org.photonvision.vision.pipeline.result;
import java.util.Collections;
import java.util.List;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.target.TrackedTarget;
@@ -30,18 +31,33 @@ public class CVPipelineResult implements Releasable {
public final double fps;
public final List<TrackedTarget> targets;
public final Frame inputAndOutputFrame;
public MultiTargetPNPResults multiTagResult;
public CVPipelineResult(
double processingNanos, double fps, List<TrackedTarget> targets, Frame inputFrame) {
this(processingNanos, fps, targets, new MultiTargetPNPResults(), inputFrame);
}
public CVPipelineResult(
double processingNanos,
double fps,
List<TrackedTarget> targets,
MultiTargetPNPResults multiTagResults,
Frame inputFrame) {
this.processingNanos = processingNanos;
this.fps = fps;
this.targets = targets != null ? targets : Collections.emptyList();
this.multiTagResult = multiTagResults;
this.inputAndOutputFrame = inputFrame;
}
public CVPipelineResult(double processingNanos, double fps, List<TrackedTarget> targets) {
this(processingNanos, fps, targets, null);
public CVPipelineResult(
double processingNanos,
double fps,
List<TrackedTarget> targets,
MultiTargetPNPResults multiTagResults) {
this(processingNanos, fps, targets, multiTagResults, null);
}
public boolean hasTargets() {

90
photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -22,10 +22,19 @@ 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.ArrayList;
import java.util.HashMap;
import java.util.List;
import org.opencv.core.*;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.RotatedRect;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
import org.photonvision.vision.aruco.ArucoDetectionResult;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.CVShape;
@@ -102,6 +111,22 @@ public class TrackedTarget implements Releasable {
m_altCameraToTarget3d = altPose;
m_poseAmbiguity = tagPose.getAmbiguity();
var tvec = new Mat(3, 1, CvType.CV_64FC1);
tvec.put(
0,
0,
new double[] {
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
});
setCameraRelativeTvec(tvec);
// Opencv expects a 3d vector with norm = angle and direction = axis
var rvec = new Mat(3, 1, CvType.CV_64FC1);
MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
setCameraRelativeRvec(rvec);
}
double[] corners = tagDetection.getCorners();
@@ -127,9 +152,11 @@ public class TrackedTarget implements Releasable {
tvec.put(
0,
0,
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ());
new double[] {
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
});
setCameraRelativeTvec(tvec);
// Opencv expects a 3d vector with norm = angle and direction = axis
@@ -356,31 +383,54 @@ public class TrackedTarget implements Releasable {
ret.put("skew", getSkew());
ret.put("area", getArea());
ret.put("ambiguity", getPoseAmbiguity());
if (getBestCameraToTarget3d() != null) {
ret.put("pose", transformToMap(getBestCameraToTarget3d()));
var bestCameraToTarget3d = getBestCameraToTarget3d();
if (bestCameraToTarget3d != null) {
ret.put("pose", SerializationUtils.transformToHashMap(bestCameraToTarget3d));
}
ret.put("fiducialId", getFiducialId());
return ret;
}
private static HashMap<String, Object> transformToMap(Transform3d transform) {
var ret = new HashMap<String, Object>();
ret.put("x", transform.getTranslation().getX());
ret.put("y", transform.getTranslation().getY());
ret.put("z", transform.getTranslation().getZ());
ret.put("qw", transform.getRotation().getQuaternion().getW());
ret.put("qx", transform.getRotation().getQuaternion().getX());
ret.put("qy", transform.getRotation().getQuaternion().getY());
ret.put("qz", transform.getRotation().getQuaternion().getZ());
ret.put("angle_z", transform.getRotation().getZ());
return ret;
}
public boolean isFiducial() {
return this.m_fiducialId >= 0;
}
public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
var ret = new ArrayList<PhotonTrackedTarget>();
for (var t : targets) {
var minAreaRectCorners = new ArrayList<TargetCorner>();
var detectedCorners = new ArrayList<TargetCorner>();
{
var points = new Point[4];
t.getMinAreaRect().points(points);
for (int i = 0; i < 4; i++) {
minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y));
}
}
{
var points = t.getTargetCorners();
for (int i = 0; i < points.size(); i++) {
detectedCorners.add(new TargetCorner(points.get(i).x, points.get(i).y));
}
}
ret.add(
new PhotonTrackedTarget(
t.getYaw(),
t.getPitch(),
t.getArea(),
t.getSkew(),
t.getFiducialId(),
t.getBestCameraToTarget3d(),
t.getAltCameraToTarget3d(),
t.getPoseAmbiguity(),
minAreaRectCorners,
detectedCorners));
}
return ret;
}
public static class TargetCalculationParameters {
// TargetOffset calculation values
final boolean isLandscape;

10
photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
View File

@@ -26,6 +26,7 @@ package org.photonvision;
import edu.wpi.first.math.geometry.Pose3d;
import java.util.List;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonTrackedTarget;
/** An estimated pose based on pipeline result */
@@ -39,6 +40,9 @@ public class EstimatedRobotPose {
/** A list of the targets used to compute this pose */
public final List<PhotonTrackedTarget> targetsUsed;
/** The strategy actually used to produce this pose */
public final PoseStrategy strategy;
/**
* Constructs an EstimatedRobotPose
*
@@ -46,9 +50,13 @@ public class EstimatedRobotPose {
* @param timestampSeconds timestamp of the estimate
*/
public EstimatedRobotPose(
Pose3d estimatedPose, double timestampSeconds, List<PhotonTrackedTarget> targetsUsed) {
Pose3d estimatedPose,
double timestampSeconds,
List<PhotonTrackedTarget> targetsUsed,
PoseStrategy strategy) {
this.estimatedPose = estimatedPose;
this.timestampSeconds = timestampSeconds;
this.targetsUsed = targetsUsed;
this.strategy = strategy;
}
}

34
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -24,7 +24,11 @@
package org.photonvision;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.*;
@@ -41,6 +45,7 @@ import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.RawSubscriber;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
@@ -72,6 +77,7 @@ public class PhotonCamera implements AutoCloseable {
IntegerSubscriber heartbeatEntry;
private DoubleArraySubscriber cameraIntrinsicsSubscriber;
private DoubleArraySubscriber cameraDistortionSubscriber;
private StringPublisher atflPublisher;
@Override
public void close() {
@@ -95,6 +101,7 @@ public class PhotonCamera implements AutoCloseable {
pipelineIndexRequest.close();
cameraIntrinsicsSubscriber.close();
cameraDistortionSubscriber.close();
atflPublisher.close();
}
private final String path;
@@ -114,6 +121,7 @@ public class PhotonCamera implements AutoCloseable {
Packet packet = new Packet(1);
// Existing is enough to make this multisubscriber do its thing
private final MultiSubscriber m_topicNameSubscriber;
/**
@@ -150,6 +158,10 @@ public class PhotonCamera implements AutoCloseable {
ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1);
versionEntry = photonvision_root_table.getStringTopic("version").subscribe("");
atflPublisher = photonvision_root_table.getStringTopic("apriltag_field_layout").publish();
// Save the layout locally on Rio; on reboot, should be pushed out to NT clients
atflPublisher.getTopic().setPersistent(true);
m_topicNameSubscriber =
new MultiSubscriber(
instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
@@ -319,6 +331,28 @@ public class PhotonCamera implements AutoCloseable {
return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
}
/**
* Set the Apriltag Field Layout used by all PhotonVision coprocessors that are (or might later)
* connect to this robot. The topic is marked as persistant, so even if you only call this once
* ever, it will be saved on the RoboRIO and pushed out to all NT clients when code reboots.
* PhotonVision will also store a copy of this layout locally on the coprocessor, but subscribes
* to this topic and the local copy will be updated when this function is called.
*
* @param layout The layout to use for *all* PhotonVision cameras
* @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients
* have updated their internal layouts.
*/
public boolean setApriltagFieldLayout(AprilTagFieldLayout layout) {
try {
var layout_json = new ObjectMapper().writeValueAsString(layout);
atflPublisher.set(layout_json);
return true;
} catch (JsonProcessingException e) {
MathSharedStore.reportError("Error setting ATFL in " + this.getName(), e.getStackTrace());
}
return false;
}
public Optional<Matrix<N3, N3>> getCameraMatrix() {
var cameraMatrix = cameraIntrinsicsSubscriber.get();
if (cameraMatrix != null && cameraMatrix.length == 9) {

79
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -68,8 +68,17 @@ public class PhotonPoseEstimator {
/** Return the average of the best target poses using ambiguity as weight. */
AVERAGE_BEST_TARGETS,
/** Use all visible tags to compute a single pose estimate.. */
MULTI_TAG_PNP
/**
* Use all visible tags to compute a single pose estimate on coprocessor. This option needs to
* be enabled on the PhotonVision web UI as well.
*/
MULTI_TAG_PNP_ON_COPROCESSOR,
/**
* Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can
* take a lot of time.
*/
MULTI_TAG_PNP_ON_RIO
}
private AprilTagFieldLayout fieldTags;
@@ -173,7 +182,8 @@ public class PhotonPoseEstimator {
*/
public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
checkUpdate(this.multiTagFallbackStrategy, strategy);
if (strategy == PoseStrategy.MULTI_TAG_PNP) {
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
|| strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) {
DriverStation.reportWarning(
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false);
strategy = PoseStrategy.LOWEST_AMBIGUITY;
@@ -357,8 +367,11 @@ public class PhotonPoseEstimator {
case AVERAGE_BEST_TARGETS:
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
case MULTI_TAG_PNP:
estimatedPose = multiTagPNPStrategy(cameraResult, cameraMatrix, distCoeffs);
case MULTI_TAG_PNP_ON_RIO:
estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs);
break;
default:
DriverStation.reportError(
@@ -373,7 +386,28 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> multiTagPNPStrategy(
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
if (result.getMultiTagResult().estimatedPose.isPresent) {
var best_tf = result.getMultiTagResult().estimatedPose.best;
var best =
new Pose3d()
.plus(best_tf) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
} else {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}
}
private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
@@ -395,7 +429,11 @@ public class PhotonPoseEstimator {
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(
new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets()));
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_RIO));
}
/**
@@ -440,7 +478,8 @@ public class PhotonPoseEstimator {
.transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets()));
result.getTargets(),
PoseStrategy.LOWEST_AMBIGUITY));
}
/**
@@ -494,7 +533,8 @@ public class PhotonPoseEstimator {
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets());
result.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
if (bestTransformDelta < smallestHeightDifference) {
@@ -506,7 +546,8 @@ public class PhotonPoseEstimator {
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets());
result.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
}
@@ -568,13 +609,19 @@ public class PhotonPoseEstimator {
smallestPoseDelta = altDifference;
lowestDeltaPose =
new EstimatedRobotPose(
altTransformPosition, result.getTimestampSeconds(), result.getTargets());
altTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
if (bestDifference < smallestPoseDelta) {
smallestPoseDelta = bestDifference;
lowestDeltaPose =
new EstimatedRobotPose(
bestTransformPosition, result.getTimestampSeconds(), result.getTargets());
bestTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
}
return Optional.ofNullable(lowestDeltaPose);
@@ -617,7 +664,8 @@ public class PhotonPoseEstimator {
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets()));
result.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}
totalAmbiguity += 1.0 / target.getPoseAmbiguity();
@@ -649,7 +697,10 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
new Pose3d(transform, rotation), result.getTimestampSeconds(), result.getTargets()));
new Pose3d(transform, rotation),
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}
/**

419
photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java
View File

@@ -1,419 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Optional;
import java.util.Set;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
/**
* @deprecated Use {@link PhotonPoseEstimator}
*/
@Deprecated
public class RobotPoseEstimator {
/**
*
*
* <ul>
* <li><strong>LOWEST_AMBIGUITY</strong>: Choose the Pose with the lowest ambiguity
* <li><strong>CLOSEST_TO_CAMERA_HEIGHT</strong>: Choose the Pose which is closest to the camera
* height
* <li><strong>CLOSEST_TO_REFERENCE_POSE</strong>: Choose the Pose which is closest to the pose
* from setReferencePose()
* <li><strong>CLOSEST_TO_LAST_POSE</strong>: Choose the Pose which is closest to the last pose
* calculated
* </ul>
*/
public enum PoseStrategy {
LOWEST_AMBIGUITY,
CLOSEST_TO_CAMERA_HEIGHT,
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS
}
private AprilTagFieldLayout aprilTags;
private PoseStrategy strategy;
private List<Pair<PhotonCamera, Transform3d>> cameras;
private Pose3d lastPose;
private Pose3d referencePose;
private Set<Integer> reportedErrors;
/**
* Create a new RobotPoseEstimator.
*
* @param aprilTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from
* the center of the robot to the camera mount positions (ie, robot ➔ camera).
*/
public RobotPoseEstimator(
AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
List<Pair<PhotonCamera, Transform3d>> cameras) {
this.aprilTags = aprilTags;
this.strategy = strategy;
this.cameras = cameras;
lastPose = new Pose3d();
reportedErrors = new HashSet<>();
}
/**
* Update the estimated pose using the selected strategy.
*
* @return The updated estimated pose and the latency in milliseconds. Estimated pose may be null
* if no targets were seen
*/
public Optional<Pair<Pose3d, Double>> update() {
if (cameras.isEmpty()) {
DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false);
return Optional.empty();
}
Pair<Pose3d, Double> pair = getResultFromActiveStrategy();
if (pair != null) {
lastPose = pair.getFirst();
}
return Optional.ofNullable(pair);
}
private Pair<Pose3d, Double> getResultFromActiveStrategy() {
switch (strategy) {
case LOWEST_AMBIGUITY:
return lowestAmbiguityStrategy();
case CLOSEST_TO_CAMERA_HEIGHT:
return closestToCameraHeightStrategy();
case CLOSEST_TO_REFERENCE_POSE:
return closestToReferencePoseStrategy();
case CLOSEST_TO_LAST_POSE:
return closestToLastPoseStrategy();
case AVERAGE_BEST_TARGETS:
return averageBestTargetsStrategy();
default:
DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false);
return null;
}
}
private Pair<Pose3d, Double> lowestAmbiguityStrategy() {
int lowestAI = -1;
int lowestAJ = -1;
double lowestAmbiguityScore = 10;
ArrayList<PhotonPipelineResult> results = new ArrayList<PhotonPipelineResult>(cameras.size());
// Sample result from each camera
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
results.add(p.getFirst().getLatestResult());
}
// Loop over each ambiguity of all the cameras
for (int i = 0; i < cameras.size(); i++) {
List<PhotonTrackedTarget> targets = results.get(i).targets;
for (int j = 0; j < targets.size(); j++) {
if (targets.get(j).getPoseAmbiguity() < lowestAmbiguityScore) {
lowestAI = i;
lowestAJ = j;
lowestAmbiguityScore = targets.get(j).getPoseAmbiguity();
}
}
}
// No targets, return null
if (lowestAI == -1 || lowestAJ == -1) {
return null;
}
// Pick the lowest and do the heavy calculations
PhotonTrackedTarget bestTarget = results.get(lowestAI).targets.get(lowestAJ);
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(bestTarget.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(bestTarget.getFiducialId());
return null;
}
return Pair.of(
fiducialPose
.get()
.transformBy(bestTarget.getBestCameraToTarget().inverse())
.transformBy(cameras.get(lowestAI).getSecond().inverse()),
results.get(lowestAI).getLatencyMillis());
}
private Pair<Pose3d, Double> closestToCameraHeightStrategy() {
double smallestHeightDifference = Double.MAX_VALUE;
double latency = 0;
Pose3d pose = null;
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
var result = p.getFirst().getLatestResult();
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
Pose3d targetPose = fiducialPose.get();
double alternativeDifference =
Math.abs(
p.getSecond().getZ()
- targetPose.transformBy(target.getAlternateCameraToTarget().inverse()).getZ());
double bestDifference =
Math.abs(
p.getSecond().getZ()
- targetPose.transformBy(target.getBestCameraToTarget().inverse()).getZ());
if (alternativeDifference < smallestHeightDifference) {
smallestHeightDifference = alternativeDifference;
pose =
targetPose
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
latency = result.getLatencyMillis();
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose =
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
latency = result.getLatencyMillis();
}
}
}
return Pair.of(pose, latency);
}
private Pair<Pose3d, Double> closestToReferencePoseStrategy() {
if (referencePose == null) {
DriverStation.reportError(
"[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!",
false);
return null;
}
double smallestDifference = 10e9;
double latency = 0;
Pose3d pose = null;
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
var result = p.getFirst().getLatestResult();
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
Pose3d targetPose = fiducialPose.get();
Pose3d botBestPose =
targetPose
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
Pose3d botAltPose =
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
double alternativeDifference = Math.abs(calculateDifference(referencePose, botAltPose));
double bestDifference = Math.abs(calculateDifference(referencePose, botBestPose));
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
pose = botAltPose;
latency = result.getLatencyMillis();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = botBestPose;
latency = result.getLatencyMillis();
}
}
}
return Pair.of(pose, latency);
}
private Pair<Pose3d, Double> closestToLastPoseStrategy() {
setReferencePose(lastPose);
return closestToReferencePoseStrategy();
}
/** Return the average of the best target poses using ambiguity as weight */
private Pair<Pose3d, Double> averageBestTargetsStrategy() {
// Pair of Double, Double = Ambiguity, Mili
List<Pair<Pose3d, Pair<Double, Double>>> tempPoses = new ArrayList<>();
double totalAmbiguity = 0;
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
var result = p.getFirst().getLatestResult();
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
Pose3d targetPose = fiducialPose.get();
try {
totalAmbiguity += 1. / target.getPoseAmbiguity();
} catch (ArithmeticException e) {
// A total ambiguity of zero exists, using that pose instead!",
return Pair.of(
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse()),
result.getLatencyMillis());
}
tempPoses.add(
Pair.of(
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse()),
Pair.of(target.getPoseAmbiguity(), result.getLatencyMillis())));
}
}
Translation3d transform = new Translation3d();
Rotation3d rotation = new Rotation3d();
double latency = 0;
if (tempPoses.isEmpty()) {
return null;
}
if (totalAmbiguity == 0) {
Pose3d p = tempPoses.get(0).getFirst();
double l = tempPoses.get(0).getSecond().getSecond();
return Pair.of(p, l);
}
for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
transform = transform.plus(pair.getFirst().getTranslation().times(weight));
rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
}
return Pair.of(new Pose3d(transform, rotation), latency);
}
/**
* Difference is defined as the vector magnitude between the two poses
*
* @return The absolute "difference" (>=0) between two Pose3ds.
*/
private double calculateDifference(Pose3d x, Pose3d y) {
return x.getTranslation().getDistance(y.getTranslation());
}
/**
* @param aprilTags the aprilTags to set
*/
public void setAprilTags(AprilTagFieldLayout aprilTags) {
this.aprilTags = aprilTags;
}
/**
* @return the aprilTags
*/
public AprilTagFieldLayout getAprilTags() {
return aprilTags;
}
/**
* @return the strategy
*/
public PoseStrategy getStrategy() {
return strategy;
}
/**
* @param strategy the strategy to set
*/
public void setStrategy(PoseStrategy strategy) {
this.strategy = strategy;
}
/**
* @return the referencePose
*/
public Pose3d getReferencePose() {
return referencePose;
}
/**
* Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
*
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose3d referencePose) {
this.referencePose = referencePose;
}
/**
* Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
*
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose2d referencePose) {
setReferencePose(new Pose3d(referencePose));
}
/**
* Update the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE
*
* @param lastPose the lastPose to set
*/
public void setLastPose(Pose3d lastPose) {
this.lastPose = lastPose;
}
private void reportFiducialPoseError(int fiducialId) {
if (!reportedErrors.contains(fiducialId)) {
DriverStation.reportError(
"[RobotPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false);
reportedErrors.add(fiducialId);
}
}
}

93
photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java
View File

@@ -1,93 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.estimation;
import edu.wpi.first.math.geometry.Transform3d;
/**
* The best estimated transformation from solvePnP, and possibly an alternate transformation
* depending on the solvePNP method. If an alternate solution is present, the ambiguity value
* represents the ratio of reprojection error in the best solution to the alternate (best /
* alternate).
*
* <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
* method.
*/
public class PNPResults {
/**
* If this result is valid. A false value indicates there was an error in estimation, and this
* result should not be used.
*/
public final boolean isPresent;
/**
* The best-fit transform. The coordinate frame of this transform depends on the method which gave
* this result.
*/
public final Transform3d best;
/** Reprojection error of the best solution, in pixels */
public final double bestReprojErr;
/**
* Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
* to the best solution.
*/
public final Transform3d alt;
/** If no alternate solution is found, this is bestReprojErr */
public final double altReprojErr;
/** If no alternate solution is found, this is 0 */
public final double ambiguity;
/** An empty (invalid) result. */
public PNPResults() {
this.isPresent = false;
this.best = new Transform3d();
this.alt = new Transform3d();
this.ambiguity = 0;
this.bestReprojErr = 0;
this.altReprojErr = 0;
}
public PNPResults(Transform3d best, double bestReprojErr) {
this(best, best, 0, bestReprojErr, bestReprojErr);
}
public PNPResults(
Transform3d best,
Transform3d alt,
double ambiguity,
double bestReprojErr,
double altReprojErr) {
this.isPresent = true;
this.best = best;
this.alt = alt;
this.ambiguity = ambiguity;
this.bestReprojErr = bestReprojErr;
this.altReprojErr = altReprojErr;
}
}

2
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -50,9 +50,9 @@ import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.PNPResults;
import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;

4
photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java
View File

@@ -35,6 +35,7 @@ import org.photonvision.PhotonCamera;
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
@@ -140,7 +141,8 @@ public class SimPhotonCamera {
targetList.sort(sortMode.getComparator());
}
PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList);
PhotonPipelineResult newResult =
new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults());
var newPacket = new Packet(newResult.getPacketSize());
newResult.populatePacket(newPacket);
ts.rawBytesEntry.set(newPacket.getData());

113
photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp Normal file
View File

@@ -0,0 +1,113 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include "photonlib/MultiTargetPNPResult.h"
namespace photonlib {
Packet& operator<<(Packet& packet, const MultiTargetPnpResult& target) {
packet << target.result;
size_t i;
for (i = 0; i < target.fiducialIdsUsed.capacity(); i++) {
if (i < target.fiducialIdsUsed.size()) {
packet << static_cast<int16_t>(target.fiducialIdsUsed[i]);
} else {
packet << static_cast<int16_t>(-1);
}
}
return packet;
}
Packet& operator>>(Packet& packet, MultiTargetPnpResult& target) {
packet >> target.result;
target.fiducialIdsUsed.clear();
for (size_t i = 0; i < target.fiducialIdsUsed.capacity(); i++) {
int16_t id = 0;
packet >> id;
if (id > -1) {
target.fiducialIdsUsed.push_back(id);
}
}
return packet;
}
// Encode a transform3d
Packet& operator<<(Packet& packet, const frc::Transform3d& transform) {
packet << transform.Translation().X().value()
<< transform.Translation().Y().value()
<< transform.Translation().Z().value()
<< transform.Rotation().GetQuaternion().W()
<< transform.Rotation().GetQuaternion().X()
<< transform.Rotation().GetQuaternion().Y()
<< transform.Rotation().GetQuaternion().Z();
return packet;
}
// Decode a transform3d
Packet& operator>>(Packet& packet, frc::Transform3d& transform) {
frc::Transform3d ret;
// We use these for best and alt transforms below
double x = 0;
double y = 0;
double z = 0;
double w = 0;
// decode and unitify translation
packet >> x >> y >> z;
const auto translation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
// decode and add units to rotation
packet >> w >> x >> y >> z;
const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
transform = frc::Transform3d(translation, rotation);
return packet;
}
Packet& operator<<(Packet& packet, PNPResults const& result) {
packet << result.isValid << result.best << result.alt
<< result.bestReprojectionErr << result.altReprojectionErr
<< result.ambiguity;
return packet;
}
Packet& operator>>(Packet& packet, PNPResults& result) {
packet >> result.isValid >> result.best >> result.alt >>
result.bestReprojectionErr >> result.altReprojectionErr >>
result.ambiguity;
return packet;
}
} // namespace photonlib

6
photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp
View File

@@ -40,7 +40,7 @@ bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const {
Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
// Encode latency and number of targets.
packet << result.latency.value() * 1000
packet << result.latency.value() * 1000 << result.m_pnpResults
<< static_cast<int8_t>(result.targets.size());
// Encode the information of each target.
@@ -52,9 +52,9 @@ Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
// Decode latency, existence of targets, and number of targets.
int8_t targetCount = 0;
double latencyMillis = 0;
packet >> latencyMillis >> targetCount;
int8_t targetCount = 0;
packet >> latencyMillis >> result.m_pnpResults >> targetCount;
result.latency = units::second_t(latencyMillis / 1000.0);
result.targets.clear();

46
photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
View File

@@ -82,7 +82,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
poseCacheTimestamp(-1_s) {}
void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
if (strategy == MULTI_TAG_PNP) {
if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR ||
strategy == MULTI_TAG_PNP_ON_RIO) {
FRC_ReportError(
frc::warn::Warning,
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
@@ -162,8 +163,12 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
case AVERAGE_BEST_TARGETS:
ret = AverageBestTargetsStrategy(result);
break;
case ::photonlib::MULTI_TAG_PNP:
ret = MultiTagPnpStrategy(result, cameraMatrixData, cameraDistCoeffs);
case MULTI_TAG_PNP_ON_COPROCESSOR:
ret =
MultiTagOnCoprocStrategy(result, cameraMatrixData, cameraDistCoeffs);
break;
case MULTI_TAG_PNP_ON_RIO:
ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -205,7 +210,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets()};
result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
}
std::optional<EstimatedRobotPose>
@@ -241,14 +246,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets()};
result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets()};
result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
}
}
@@ -299,7 +304,8 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
}
}
return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets()};
return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets(),
CLOSEST_TO_REFERENCE_POSE};
}
std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
@@ -351,7 +357,24 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
Rotation3d(rv));
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs) {
if (result.MultiTagResult().result.isValid) {
const auto field2camera = result.MultiTagResult().result.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
return photonlib::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
result.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
}
return Update(result, std::nullopt, std::nullopt,
this->multiTagFallbackStrategy);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs) {
using namespace frc;
@@ -404,7 +427,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
return photonlib::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
result.GetTargets());
result.GetTargets(), MULTI_TAG_PNP_ON_RIO);
}
std::optional<EstimatedRobotPose>
@@ -430,7 +453,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
return EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets()};
result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS};
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
@@ -450,6 +473,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
}
return EstimatedRobotPose{frc::Pose3d(transform, rotation),
result.GetTimestamp(), result.GetTargets()};
result.GetTimestamp(), result.GetTargets(),
AVERAGE_BEST_TARGETS};
}
} // namespace photonlib

282
photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
View File

@@ -1,282 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include "photonlib/RobotPoseEstimator.h"
#include <iostream>
#include <limits>
#include <map>
#include <span>
#include <string>
#include <utility>
#include <vector>
#include <frc/Errors.h>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <units/time.h>
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photonlib/PhotonTrackedTarget.h"
namespace photonlib {
RobotPoseEstimator::RobotPoseEstimator(
std::shared_ptr<frc::AprilTagFieldLayout> tags, PoseStrategy strat,
std::vector<std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>>
cams)
: aprilTags(tags),
strategy(strat),
cameras(std::move(cams)),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()) {}
std::pair<frc::Pose3d, units::second_t> RobotPoseEstimator::Update() {
if (cameras.empty()) {
return std::make_pair(lastPose, units::second_t(0));
}
std::pair<frc::Pose3d, units::second_t> pair;
switch (strategy) {
case LOWEST_AMBIGUITY:
pair = LowestAmbiguityStrategy();
lastPose = pair.first;
return pair;
case CLOSEST_TO_CAMERA_HEIGHT:
pair = ClosestToCameraHeightStrategy();
lastPose = pair.first;
return pair;
case CLOSEST_TO_REFERENCE_POSE:
pair = ClosestToReferencePoseStrategy();
lastPose = pair.first;
return pair;
case CLOSEST_TO_LAST_POSE:
SetReferencePose(lastPose);
pair = ClosestToReferencePoseStrategy();
lastPose = pair.first;
return pair;
case AVERAGE_BEST_TARGETS:
pair = AverageBestTargetsStrategy();
lastPose = pair.first;
return pair;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
"");
}
return std::make_pair(lastPose, units::second_t(0));
}
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::LowestAmbiguityStrategy() {
int lowestAI = -1;
int lowestAJ = -1;
double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
std::span<const PhotonTrackedTarget> targets =
p.first->GetLatestResult().GetTargets();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) {
lowestAI = i;
lowestAJ = j;
lowestAmbiguityScore = targets[j].GetPoseAmbiguity();
}
}
}
if (lowestAI == -1 || lowestAJ == -1) {
return std::make_pair(lastPose, units::second_t(0));
}
PhotonTrackedTarget bestTarget =
cameras[lowestAI].first->GetLatestResult().GetTargets()[lowestAJ];
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(bestTarget.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
bestTarget.GetFiducialId());
return std::make_pair(lastPose, units::second_t(0));
}
return std::make_pair(
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(cameras[lowestAI].second.Inverse()),
cameras[lowestAI].first->GetLatestResult().GetTimestamp());
}
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::ClosestToCameraHeightStrategy() {
units::meter_t smallestHeightDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
std::span<const PhotonTrackedTarget> targets =
p.first->GetLatestResult().GetTargets();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
units::meter_t alternativeDifference = units::math::abs(
p.second.Z() -
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.Z());
units::meter_t bestDifference = units::math::abs(
p.second.Z() -
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
if (alternativeDifference < smallestHeightDifference) {
smallestHeightDifference = alternativeDifference;
pose = targetPose.TransformBy(
target.GetAlternateCameraToTarget().Inverse());
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
}
}
return std::make_pair(pose, stateTimestamp);
}
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::ClosestToReferencePoseStrategy() {
units::meter_t smallestDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
std::span<const PhotonTrackedTarget> targets =
p.first->GetLatestResult().GetTargets();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
units::meter_t alternativeDifference =
units::math::abs(referencePose.Translation().Distance(
targetPose
.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.Translation()));
units::meter_t bestDifference =
units::math::abs(referencePose.Translation().Distance(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.Translation()));
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
pose = targetPose.TransformBy(
target.GetAlternateCameraToTarget().Inverse());
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
}
}
return std::make_pair(pose, stateTimestamp);
}
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::AverageBestTargetsStrategy() {
std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
tempPoses;
double totalAmbiguity = 0;
units::second_t timstampSum = units::second_t(0);
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
std::span<const PhotonTrackedTarget> targets =
p.first->GetLatestResult().GetTargets();
timstampSum += p.first->GetLatestResult().GetTimestamp();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
if (target.GetPoseAmbiguity() == 0) {
FRC_ReportError(frc::warn::Warning,
"Pose ambiguity of zero exists, using that instead!",
"");
return std::make_pair(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
p.first->GetLatestResult().GetLatency() / 1000.);
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
tempPoses.push_back(std::make_pair(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
std::make_pair(target.GetPoseAmbiguity(),
p.first->GetLatestResult().GetTimestamp())));
}
}
frc::Translation3d transform = frc::Translation3d();
frc::Rotation3d rotation = frc::Rotation3d();
for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
tempPoses) {
double weight = (1. / pair.second.first) / totalAmbiguity;
transform = transform + pair.first.Translation() * weight;
rotation = rotation + pair.first.Rotation() * weight;
}
return std::make_pair(frc::Pose3d(transform, rotation),
timstampSum / cameras.size());
}
} // namespace photonlib

61
photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h Normal file
View File

@@ -0,0 +1,61 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/geometry/Transform3d.h>
#include <wpi/SmallVector.h>
#include "photonlib/Packet.h"
namespace photonlib {
class PNPResults {
public:
// This could be wrapped in an std::optional, but chose to do it this way to
// mirror Java
bool isValid;
frc::Transform3d best;
double bestReprojectionErr;
frc::Transform3d alt;
double altReprojectionErr;
double ambiguity;
friend Packet& operator<<(Packet& packet, const PNPResults& result);
friend Packet& operator>>(Packet& packet, PNPResults& result);
};
class MultiTargetPnpResult {
public:
PNPResults result;
wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result);
friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result);
};
} // namespace photonlib

9
photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h
View File

@@ -31,6 +31,7 @@
#include <units/time.h>
#include <wpi/SmallVector.h>
#include "photonlib/MultiTargetPNPResult.h"
#include "photonlib/Packet.h"
#include "photonlib/PhotonTrackedTarget.h"
@@ -87,6 +88,13 @@ class PhotonPipelineResult {
*/
units::second_t GetTimestamp() const { return timestamp; }
/**
* Return the latest mulit-target result, as calculated on your coprocessor.
* Be sure to check getMultiTagResult().estimatedPose.isValid before using the
* pose estimate!
*/
const MultiTargetPnpResult& MultiTagResult() const { return m_pnpResults; }
/**
* Sets the timestamp in seconds
* @param timestamp The timestamp in seconds
@@ -119,6 +127,7 @@ class PhotonPipelineResult {
units::second_t latency = 0_s;
units::second_t timestamp = -1_s;
wpi::SmallVector<PhotonTrackedTarget, 10> targets;
MultiTargetPnpResult m_pnpResults;
inline static bool HAS_WARNED = false;
};
} // namespace photonlib

25
photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
View File

@@ -44,7 +44,8 @@ enum PoseStrategy {
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS,
MULTI_TAG_PNP
MULTI_TAG_PNP_ON_COPROCESSOR,
MULTI_TAG_PNP_ON_RIO,
};
struct EstimatedRobotPose {
@@ -57,11 +58,16 @@ struct EstimatedRobotPose {
/** A list of the targets used to compute this pose */
wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
/** The strategy actually used to produce this pose */
PoseStrategy strategy;
EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
std::span<const PhotonTrackedTarget> targets)
std::span<const PhotonTrackedTarget> targets,
PoseStrategy strategy_)
: estimatedPose(pose_),
timestamp(time_),
targetsUsed(targets.data(), targets.data() + targets.size()) {}
targetsUsed(targets.data(), targets.data() + targets.size()),
strategy(strategy_) {}
};
/**
@@ -260,14 +266,23 @@ class PhotonPoseEstimator {
std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
PhotonPipelineResult result);
/**
* Return the pose calculated by combining all tags into one on coprocessor
*
* @return the estimated position of the robot in the FCS
*/
std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs);
/**
* Return the pose calculation using all targets in view in the same PNP()
calculation
*
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> MultiTagPnpStrategy(
std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs);

187
photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h
View File

@@ -1,187 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <map>
#include <memory>
#include <utility>
#include <vector>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
#include "photonlib/PhotonCamera.h"
namespace frc {
class AprilTagFieldLayout;
} // namespace frc
namespace photonlib {
enum PoseStrategy : int {
LOWEST_AMBIGUITY,
CLOSEST_TO_CAMERA_HEIGHT,
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS
};
/**
* The RobotPoseEstimator class filters or combines readings from all the
* fiducials visible at a given timestamp on the field to produce a single robot
* in field pose, using the strategy set below. Example usage can be found in
* our apriltagExample example project.
*/
class RobotPoseEstimator {
public:
using map_value_type =
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
using size_type = std::vector<map_value_type>::size_type;
/**
* Create a new RobotPoseEstimator.
*
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
* <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
* at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param cameras An ArrayList of Pairs of PhotonCameras and their respective
* Transform3ds from the center of the robot to the cameras.
*/
explicit RobotPoseEstimator(
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
PoseStrategy strategy, std::vector<map_value_type> cameras);
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
* @return the AprilTagFieldLayout
*/
std::shared_ptr<frc::AprilTagFieldLayout> getFieldLayout() const {
return aprilTags;
}
/**
* Set the cameras to be used by the PoseEstimator.
*
* @param cameras cameras to set.
*/
inline void SetCameras(
const std::vector<std::pair<std::shared_ptr<PhotonCamera>,
frc::Transform3d>>& cameras) {
this->cameras = cameras;
}
/**
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
*/
PoseStrategy GetPoseStrategy() const { return strategy; }
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
*/
inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
*/
frc::Pose3d GetReferencePose() const { return referencePose; }
/**
* Update the stored reference pose for use when using the
* CLOSEST_TO_REFERENCE_POSE strategy.
*
* @param referencePose the referencePose to set
*/
inline void SetReferencePose(frc::Pose3d referencePose) {
this->referencePose = referencePose;
}
/**
* Update the stored last pose. Useful for setting the initial estimate when
* using the CLOSEST_TO_LAST_POSE strategy.
*
* @param lastPose the lastPose to set
*/
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
std::pair<frc::Pose3d, units::second_t> Update();
private:
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
PoseStrategy strategy;
std::vector<map_value_type> cameras;
frc::Pose3d lastPose;
frc::Pose3d referencePose;
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::pair<frc::Pose3d, units::second_t> LowestAmbiguityStrategy();
/**
* Return the estimated position of the robot using the target with the lowest
* delta height difference between the estimated and actual height of the
* camera.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::pair<frc::Pose3d, units::second_t> ClosestToCameraHeightStrategy();
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::pair<frc::Pose3d, units::second_t> ClosestToReferencePoseStrategy();
/**
* Return the average of the best target poses using ambiguity as weight.
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this
* estimation.
*/
std::pair<frc::Pose3d, units::second_t> AverageBestTargetsStrategy();
};
} // namespace photonlib

60
photon-lib/src/test/java/org/photonvision/PacketTest.java
View File

@@ -30,6 +30,8 @@ import java.util.List;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.Test;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
@@ -127,4 +129,62 @@ class PacketTest {
Assertions.assertEquals(result2, b2);
}
@Test
public void testMultiTargetSerde() {
var result =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
2,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))),
new MultiTargetPNPResults(
new PNPResults(
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(1, 2, 3)));
Packet packet = new Packet(result.getPacketSize());
result.populatePacket(packet);
var result_deserialized = new PhotonPipelineResult();
result_deserialized.createFromPacket(packet);
Assertions.assertEquals(result, result_deserialized);
}
}

5
photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java
View File

@@ -84,7 +84,10 @@ public class ApriltagWorkbenchTest {
var pe =
new PhotonPoseEstimator(
tagLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, cam, robotToCamera);
tagLayout,
PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
cam,
robotToCamera);
var field = new Field2d();
SmartDashboard.putData(field);

442
photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp
View File

@@ -1,442 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <map>
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <units/angle.h>
#include <units/length.h>
#include <wpi/SmallVector.h>
#include "gtest/gtest.h"
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photonlib/PhotonTrackedTarget.h"
#include "photonlib/RobotPoseEstimator.h"
static wpi::SmallVector<std::pair<double, double>, 4> corners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
static std::vector<std::pair<double, double>> detectedCorners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
cameras;
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
std::make_shared<photonlib::PhotonCamera>("test");
std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
std::make_shared<photonlib::PhotonCamera>("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(11));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraTwo->testResult.SetTimestamp(units::second_t(units::second_t(16)));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
cameras.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
photonlib::RobotPoseEstimator estimator(aprilTags,
photonlib::LOWEST_AMBIGUITY, cameras);
std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
}
TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())},
};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
cameras;
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
std::make_shared<photonlib::PhotonCamera>("test");
std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
std::make_shared<photonlib::PhotonCamera>("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(4));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraOne->testResult.SetTimestamp(units::second_t(12));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 4_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
cameras.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
photonlib::RobotPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, cameras);
std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(12, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.Z()), .01);
}
TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())},
};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
cameras;
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
std::make_shared<photonlib::PhotonCamera>("test");
std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
std::make_shared<photonlib::PhotonCamera>("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(4));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraTwo->testResult.SetTimestamp(units::second_t(17));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
cameras.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
photonlib::RobotPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras);
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
}
TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
cameras;
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
std::make_shared<photonlib::PhotonCamera>("test");
std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
std::make_shared<photonlib::PhotonCamera>("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
cameras.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
photonlib::RobotPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_LAST_POSE, cameras);
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targetsThree{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0,
frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->testResult = {2_s, targetsThree};
cameraOne->testResult.SetTimestamp(units::second_t(7));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsFour{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraTwo->testResult = {4_s, targetsFour};
cameraTwo->testResult.SetTimestamp(units::second_t(13));
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
camerasUpdated;
camerasUpdated.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
camerasUpdated.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
estimator.SetCameras(camerasUpdated);
estimatedPose = estimator.Update();
pose = estimatedPose.first;
EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
.01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
}
TEST(RobotPoseEstimatorTest, AverageBestPoses) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
cameras;
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
std::make_shared<photonlib::PhotonCamera>("test");
std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
std::make_shared<photonlib::PhotonCamera>("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(10));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraTwo->testResult.SetTimestamp(units::second_t(20));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
cameras.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
photonlib::RobotPoseEstimator estimator(
aprilTags, photonlib::AVERAGE_BEST_TARGETS, cameras);
std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
.01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
}

42
photon-server/src/main/java/org/photonvision/server/RequestHandler.java
View File

@@ -246,6 +246,48 @@ public class RequestHandler {
}
}
public static void onAprilTagFieldLayoutRequest(Context ctx) {
var file = ctx.uploadedFile("data");
if (file == null) {
ctx.status(400);
ctx.result(
"No File was sent with the request. Make sure that the field layout json is sent at the key 'data'");
logger.error(
"No File was sent with the request. Make sure that the field layout json is sent at the key 'data'");
return;
}
if (!file.extension().contains("json")) {
ctx.status(400);
ctx.result(
"The uploaded file was not of type 'json'. The uploaded file should be a .json file.");
logger.error(
"The uploaded file was not of type 'json'. The uploaded file should be a .json file.");
return;
}
// Create a temp file
var tempFilePath = handleTempFileCreation(file);
if (tempFilePath.isEmpty()) {
ctx.status(500);
ctx.result("There was an error while creating a temporary copy of the file");
logger.error("There was an error while creating a temporary copy of the file");
return;
}
if (ConfigManager.getInstance().saveUploadedAprilTagFieldLayout(tempFilePath.get().toPath())) {
ctx.status(200);
ctx.result("Successfully saved the uploaded AprilTagFieldLayout");
logger.info("Successfully saved the uploaded AprilTagFieldLayout");
} else {
ctx.status(500);
ctx.result("There was an error while saving the uploaded AprilTagFieldLayout");
logger.error("There was an error while saving the uploaded AprilTagFieldLayout");
}
}
public static void onOfflineUpdateRequest(Context ctx) {
var file = ctx.uploadedFile("jarData");

1
photon-server/src/main/java/org/photonvision/server/Server.java
View File

@@ -97,6 +97,7 @@ public class Server {
app.post("/api/settings/hardwareConfig", RequestHandler::onHardwareConfigRequest);
app.post("/api/settings/hardwareSettings", RequestHandler::onHardwareSettingsRequest);
app.post("/api/settings/networkConfig", RequestHandler::onNetworkConfigRequest);
app.post("/api/settings/aprilTagFieldLayout", RequestHandler::onAprilTagFieldLayoutRequest);
app.post("/api/settings/general", RequestHandler::onGeneralSettingsRequest);
app.post("/api/settings/camera", RequestHandler::onCameraSettingsRequest);
app.post("/api/settings/camera/setNickname", RequestHandler::onCameraNicknameChangeRequest);

1
photon-targeting/build.gradle
View File

@@ -4,6 +4,7 @@ apply from: "${rootDir}/shared/common.gradle"
dependencies {
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation wpilibTools.deps.wpilibJava("apriltag")
implementation "org.ejml:ejml-simple:0.42"
// Junit

17
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
View File

@@ -85,6 +85,16 @@ public class Packet {
packetData[writePos++] = src;
}
/**
* Encodes the short into the packet.
*
* @param src The short to encode.
*/
public void encode(short src) {
packetData[writePos++] = (byte) (src >>> 8);
packetData[writePos++] = (byte) src;
}
/**
* Encodes the integer into the packet.
*
@@ -196,4 +206,11 @@ public class Packet {
}
return ret;
}
public short decodeShort() {
if (packetData.length < readPos + 1) {
return 0;
}
return (short) ((0xff & packetData[readPos++]) << 8 | (0xff & packetData[readPos++]));
}
}

29
photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java → photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java
View File

@@ -1,25 +1,18 @@
/*
* MIT License
* Copyright (C) Photon Vision.
*
* Copyright (c) PhotonVision
* 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.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 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.
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
* 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.estimation;

30
photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java → photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View File

@@ -1,25 +1,18 @@
/*
* MIT License
* Copyright (C) Photon Vision.
*
* Copyright (c) PhotonVision
* 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.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 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.
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
* 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.estimation;
@@ -52,6 +45,7 @@ import org.opencv.core.Point3;
import org.opencv.core.Rect;
import org.opencv.core.RotatedRect;
import org.opencv.imgproc.Imgproc;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.TargetCorner;
public final class OpenCVHelp {

29
photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java → photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
View File

@@ -1,25 +1,18 @@
/*
* MIT License
* Copyright (C) Photon Vision.
*
* Copyright (c) PhotonVision
* 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.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 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.
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
* 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.estimation;

29
photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java → photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java
View File

@@ -1,25 +1,18 @@
/*
* MIT License
* Copyright (C) Photon Vision.
*
* Copyright (c) PhotonVision
* 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.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 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.
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
* 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.estimation;

30
photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java → photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
View File

@@ -1,25 +1,18 @@
/*
* MIT License
* Copyright (C) Photon Vision.
*
* Copyright (c) PhotonVision
* 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.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 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.
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
* 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.estimation;
@@ -36,6 +29,7 @@ import java.util.List;
import java.util.Objects;
import java.util.stream.Collectors;
import org.opencv.core.Point;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;

93
photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java Normal file
View File

@@ -0,0 +1,93 @@
/*
* 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.targeting;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
public class MultiTargetPNPResults {
// Seeing 32 apriltags at once seems like a sane limit
private static final int MAX_IDS = 32;
// pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS);
public PNPResults estimatedPose = new PNPResults();
public List<Integer> fiducialIDsUsed = List.of();
public MultiTargetPNPResults() {}
public MultiTargetPNPResults(PNPResults results, List<Integer> ids) {
estimatedPose = results;
fiducialIDsUsed = ids;
}
public static MultiTargetPNPResults createFromPacket(Packet packet) {
var results = PNPResults.createFromPacket(packet);
var ids = new ArrayList<Integer>(MAX_IDS);
for (int i = 0; i < MAX_IDS; i++) {
int targetId = (int) packet.decodeShort();
if (targetId > -1) ids.add(targetId);
}
return new MultiTargetPNPResults(results, ids);
}
public void populatePacket(Packet packet) {
estimatedPose.populatePacket(packet);
for (int i = 0; i < MAX_IDS; i++) {
if (i < fiducialIDsUsed.size()) {
packet.encode((short) fiducialIDsUsed.get(i).byteValue());
} else {
packet.encode((short) -1);
}
}
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + ((estimatedPose == null) ? 0 : estimatedPose.hashCode());
result = prime * result + ((fiducialIDsUsed == null) ? 0 : fiducialIDsUsed.hashCode());
return result;
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
MultiTargetPNPResults other = (MultiTargetPNPResults) obj;
if (estimatedPose == null) {
if (other.estimatedPose != null) return false;
} else if (!estimatedPose.equals(other.estimatedPose)) return false;
if (fiducialIDsUsed == null) {
if (other.fiducialIDsUsed != null) return false;
} else if (!fiducialIDsUsed.equals(other.fiducialIDsUsed)) return false;
return true;
}
@Override
public String toString() {
return "MultiTargetPNPResults [estimatedPose="
+ estimatedPose
+ ", fiducialIDsUsed="
+ fiducialIDsUsed
+ "]";
}
}

170
photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java Normal file
View File

@@ -0,0 +1,170 @@
/*
* 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.targeting;
import edu.wpi.first.math.geometry.Transform3d;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.utils.PacketUtils;
/**
* The best estimated transformation from solvePnP, and possibly an alternate transformation
* depending on the solvePNP method. If an alternate solution is present, the ambiguity value
* represents the ratio of reprojection error in the best solution to the alternate (best /
* alternate).
*
* <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
* method.
*/
public class PNPResults {
/**
* If this result is valid. A false value indicates there was an error in estimation, and this
* result should not be used.
*/
public final boolean isPresent;
/**
* The best-fit transform. The coordinate frame of this transform depends on the method which gave
* this result.
*/
public final Transform3d best;
/** Reprojection error of the best solution, in pixels */
public final double bestReprojErr;
/**
* Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
* to the best solution.
*/
public final Transform3d alt;
/** If no alternate solution is found, this is bestReprojErr */
public final double altReprojErr;
/** If no alternate solution is found, this is 0 */
public final double ambiguity;
/** An empty (invalid) result. */
public PNPResults() {
this.isPresent = false;
this.best = new Transform3d();
this.alt = new Transform3d();
this.ambiguity = 0;
this.bestReprojErr = 0;
this.altReprojErr = 0;
}
public PNPResults(Transform3d best, double bestReprojErr) {
this(best, best, 0, bestReprojErr, bestReprojErr);
}
public PNPResults(
Transform3d best,
Transform3d alt,
double ambiguity,
double bestReprojErr,
double altReprojErr) {
this.isPresent = true;
this.best = best;
this.alt = alt;
this.ambiguity = ambiguity;
this.bestReprojErr = bestReprojErr;
this.altReprojErr = altReprojErr;
}
public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3);
public static PNPResults createFromPacket(Packet packet) {
var present = packet.decodeBoolean();
var best = PacketUtils.decodeTransform(packet);
var alt = PacketUtils.decodeTransform(packet);
var bestEr = packet.decodeDouble();
var altEr = packet.decodeDouble();
var ambiguity = packet.decodeDouble();
if (present) {
return new PNPResults(best, alt, ambiguity, bestEr, altEr);
} else {
return new PNPResults();
}
}
public Packet populatePacket(Packet packet) {
packet.encode(isPresent);
PacketUtils.encodeTransform(packet, best);
PacketUtils.encodeTransform(packet, alt);
packet.encode(bestReprojErr);
packet.encode(altReprojErr);
packet.encode(ambiguity);
return packet;
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (isPresent ? 1231 : 1237);
result = prime * result + ((best == null) ? 0 : best.hashCode());
long temp;
temp = Double.doubleToLongBits(bestReprojErr);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + ((alt == null) ? 0 : alt.hashCode());
temp = Double.doubleToLongBits(altReprojErr);
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(ambiguity);
result = prime * result + (int) (temp ^ (temp >>> 32));
return result;
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PNPResults other = (PNPResults) obj;
if (isPresent != other.isPresent) return false;
if (best == null) {
if (other.best != null) return false;
} else if (!best.equals(other.best)) return false;
if (Double.doubleToLongBits(bestReprojErr) != Double.doubleToLongBits(other.bestReprojErr))
return false;
if (alt == null) {
if (other.alt != null) return false;
} else if (!alt.equals(other.alt)) return false;
if (Double.doubleToLongBits(altReprojErr) != Double.doubleToLongBits(other.altReprojErr))
return false;
if (Double.doubleToLongBits(ambiguity) != Double.doubleToLongBits(other.ambiguity))
return false;
return true;
}
@Override
public String toString() {
return "PNPResults [isPresent="
+ isPresent
+ ", best="
+ best
+ ", bestReprojErr="
+ bestReprojErr
+ ", alt="
+ alt
+ ", altReprojErr="
+ altReprojErr
+ ", ambiguity="
+ ambiguity
+ "]";
}
}

49
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
View File

@@ -34,6 +34,9 @@ public class PhotonPipelineResult {
// Timestamp in milliseconds.
private double timestampSeconds = -1;
// Multi-tag result
private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
/** Constructs an empty pipeline result. */
public PhotonPipelineResult() {}
@@ -48,13 +51,30 @@ public class PhotonPipelineResult {
this.targets.addAll(targets);
}
/**
* Constructs a pipeline result.
*
* @param latencyMillis The latency in the pipeline.
* @param targets The list of targets identified by the pipeline.
* @param result Result from multi-target PNP.
*/
public PhotonPipelineResult(
double latencyMillis, List<PhotonTrackedTarget> targets, MultiTargetPNPResults result) {
this.latencyMillis = latencyMillis;
this.targets.addAll(targets);
this.multiTagResult = result;
}
/**
* Returns the size of the packet needed to store this pipeline result.
*
* @return The size of the packet needed to store this pipeline result.
*/
public int getPacketSize() {
return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES + 8 + 2;
return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES
+ 8 // latency
+ MultiTargetPNPResults.PACK_SIZE_BYTES
+ 1; // target count
}
/**
@@ -122,6 +142,14 @@ public class PhotonPipelineResult {
return new ArrayList<>(targets);
}
/**
* Return the latest mulit-target result. Be sure to check
* getMultiTagResult().estimatedPose.isPresent before using the pose estimate!
*/
public MultiTargetPNPResults getMultiTagResult() {
return multiTagResult;
}
/**
* Populates the fields of the pipeline result from the packet.
*
@@ -131,6 +159,7 @@ public class PhotonPipelineResult {
public Packet createFromPacket(Packet packet) {
// Decode latency, existence of targets, and number of targets.
latencyMillis = packet.decodeDouble();
this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet);
byte targetCount = packet.decodeByte();
targets.clear();
@@ -154,6 +183,7 @@ public class PhotonPipelineResult {
public Packet populatePacket(Packet packet) {
// Encode latency, existence of targets, and number of targets.
packet.encode(latencyMillis);
multiTagResult.populatePacket(packet);
packet.encode((byte) targets.size());
// Encode the information of each target.
@@ -173,6 +203,7 @@ public class PhotonPipelineResult {
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(timestampSeconds);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode());
return result;
}
@@ -189,6 +220,22 @@ public class PhotonPipelineResult {
return false;
if (Double.doubleToLongBits(timestampSeconds)
!= Double.doubleToLongBits(other.timestampSeconds)) return false;
if (multiTagResult == null) {
if (other.multiTagResult != null) return false;
} else if (!multiTagResult.equals(other.multiTagResult)) return false;
return true;
}
@Override
public String toString() {
return "PhotonPipelineResult [targets="
+ targets
+ ", latencyMillis="
+ latencyMillis
+ ", timestampSeconds="
+ timestampSeconds
+ ", multiTagResult="
+ multiTagResult
+ "]";
}
}

35
photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View File

@@ -17,13 +17,11 @@
package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.utils.PacketUtils;
public class PhotonTrackedTarget {
private static final int MAX_CORNERS = 8;
@@ -198,29 +196,6 @@ public class PhotonTrackedTarget {
return true;
}
private static Transform3d decodeTransform(Packet packet) {
double x = packet.decodeDouble();
double y = packet.decodeDouble();
double z = packet.decodeDouble();
var translation = new Translation3d(x, y, z);
double w = packet.decodeDouble();
x = packet.decodeDouble();
y = packet.decodeDouble();
z = packet.decodeDouble();
var rotation = new Rotation3d(new Quaternion(w, x, y, z));
return new Transform3d(translation, rotation);
}
private static void encodeTransform(Packet packet, Transform3d transform) {
packet.encode(transform.getTranslation().getX());
packet.encode(transform.getTranslation().getY());
packet.encode(transform.getTranslation().getZ());
packet.encode(transform.getRotation().getQuaternion().getW());
packet.encode(transform.getRotation().getQuaternion().getX());
packet.encode(transform.getRotation().getQuaternion().getY());
packet.encode(transform.getRotation().getQuaternion().getZ());
}
private static void encodeList(Packet packet, List<TargetCorner> list) {
packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE));
for (int i = 0; i < list.size(); i++) {
@@ -253,8 +228,8 @@ public class PhotonTrackedTarget {
this.skew = packet.decodeDouble();
this.fiducialId = packet.decodeInt();
this.bestCameraToTarget = decodeTransform(packet);
this.altCameraToTarget = decodeTransform(packet);
this.bestCameraToTarget = PacketUtils.decodeTransform(packet);
this.altCameraToTarget = PacketUtils.decodeTransform(packet);
this.poseAmbiguity = packet.decodeDouble();
@@ -282,8 +257,8 @@ public class PhotonTrackedTarget {
packet.encode(area);
packet.encode(skew);
packet.encode(fiducialId);
encodeTransform(packet, bestCameraToTarget);
encodeTransform(packet, altCameraToTarget);
PacketUtils.encodeTransform(packet, bestCameraToTarget);
PacketUtils.encodeTransform(packet, altCameraToTarget);
packet.encode(poseAmbiguity);
for (int i = 0; i < 4; i++) {

49
photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java Normal file
View File

@@ -0,0 +1,49 @@
/*
* 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.utils;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import org.photonvision.common.dataflow.structures.Packet;
public class PacketUtils {
public static Transform3d decodeTransform(Packet packet) {
double x = packet.decodeDouble();
double y = packet.decodeDouble();
double z = packet.decodeDouble();
var translation = new Translation3d(x, y, z);
double w = packet.decodeDouble();
x = packet.decodeDouble();
y = packet.decodeDouble();
z = packet.decodeDouble();
var rotation = new Rotation3d(new Quaternion(w, x, y, z));
return new Transform3d(translation, rotation);
}
public static void encodeTransform(Packet packet, Transform3d transform) {
packet.encode(transform.getTranslation().getX());
packet.encode(transform.getTranslation().getY());
packet.encode(transform.getTranslation().getZ());
packet.encode(transform.getRotation().getQuaternion().getW());
packet.encode(transform.getRotation().getQuaternion().getX());
packet.encode(transform.getRotation().getQuaternion().getY());
packet.encode(transform.getRotation().getQuaternion().getZ());
}
}

4
photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
View File

@@ -36,8 +36,8 @@ class PhotonCameraWrapper {
public:
photonlib::PhotonPoseEstimator m_poseEstimator{
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
photonlib::MULTI_TAG_PNP, std::move(photonlib::PhotonCamera{"WPI2023"}),
frc::Transform3d{}};
photonlib::MULTI_TAG_PNP_ON_RIO,
std::move(photonlib::PhotonCamera{"WPI2023"}), frc::Transform3d{}};
inline std::optional<photonlib::EstimatedRobotPose> Update(
frc::Pose2d estimatedPose) {

3
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java
View File

@@ -56,7 +56,8 @@ public class Vision {
camera = new PhotonCamera(kCameraName);
photonEstimator =
new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP, camera, kRobotToCam);
new PhotonPoseEstimator(
kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, kRobotToCam);
photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
// ----- Simulation