From 47bd077bbb2790bacb84d5615629e625359930a6 Mon Sep 17 00:00:00 2001 From: Matt Date: Tue, 17 Oct 2023 10:20:00 -0400 Subject: [PATCH] Run multitag on coprocessor (#816) --- .gitignore | 1 - photon-client/src/App.vue | 2 +- .../src/components/app/photon-log-view.vue | 2 +- .../src/components/cameras/CamerasView.vue | 6 +- .../src/components/dashboard/CamerasCard.vue | 6 +- .../components/dashboard/tabs/Map3DTab.vue | 2 +- .../components/dashboard/tabs/OutputTab.vue | 28 +- .../components/dashboard/tabs/TargetsTab.vue | 34 +- .../settings/ApriltagControlCard.vue | 93 ++++ .../components/settings/DeviceControlCard.vue | 14 +- .../src/components/settings/MetricsCard.vue | 4 +- photon-client/src/stores/StateStore.ts | 20 +- .../stores/settings/CameraSettingsStore.ts | 4 +- .../stores/settings/GeneralSettingsStore.ts | 10 + .../src/types/PhotonTrackingTypes.ts | 41 +- photon-client/src/types/PipelineTypes.ts | 7 +- photon-client/src/types/WebsocketDataTypes.ts | 32 +- .../src/views/GeneralSettingsView.vue | 2 + .../common/configuration/ConfigManager.java | 4 + .../common/configuration/ConfigProvider.java | 2 + .../configuration/LegacyConfigProvider.java | 46 +- .../configuration/PhotonConfiguration.java | 37 +- .../configuration/SqlConfigProvider.java | 32 +- .../networktables/NTDataPublisher.java | 44 +- .../networktables/NetworkTablesManager.java | 83 +++- .../dataflow/websocket/UIDataPublisher.java | 28 +- .../common/hardware/HardwareManager.java | 2 + .../common/networking/RoborioFinder.java | 2 +- .../common/util/SerializationUtils.java | 15 + .../vision/calibration/JsonMat.java | 16 +- .../pipe/impl/AprilTagDetectionPipe.java | 6 - .../pipe/impl/AprilTagPoseEstimatorPipe.java | 6 - .../vision/pipe/impl/MultiTargetPNPPipe.java | 88 ++++ .../vision/pipeline/AprilTagPipeline.java | 137 ++++-- .../pipeline/AprilTagPipelineSettings.java | 51 +- .../vision/pipeline/Calibrate3dPipeline.java | 2 + .../pipeline/result/CVPipelineResult.java | 20 +- .../vision/target/TrackedTarget.java | 90 +++- .../org/photonvision/EstimatedRobotPose.java | 10 +- .../java/org/photonvision/PhotonCamera.java | 34 ++ .../org/photonvision/PhotonPoseEstimator.java | 79 +++- .../org/photonvision/RobotPoseEstimator.java | 419 ----------------- .../photonvision/estimation/PNPResults.java | 93 ---- .../simulation/PhotonCameraSim.java | 2 +- .../simulation/SimPhotonCamera.java | 4 +- .../cpp/photonlib/MultiTargetPNPResult.cpp | 113 +++++ .../cpp/photonlib/PhotonPipelineResult.cpp | 6 +- .../cpp/photonlib/PhotonPoseEstimator.cpp | 46 +- .../cpp/photonlib/RobotPoseEstimator.cpp | 282 ----------- .../include/photonlib/MultiTargetPNPResult.h | 61 +++ .../include/photonlib/PhotonPipelineResult.h | 9 + .../include/photonlib/PhotonPoseEstimator.h | 25 +- .../include/photonlib/RobotPoseEstimator.h | 187 -------- .../java/org/photonvision/PacketTest.java | 60 +++ .../estimation/ApriltagWorkbenchTest.java | 5 +- .../native/cpp/RobotPoseEstimatorTest.cpp | 442 ------------------ .../photonvision/server/RequestHandler.java | 42 ++ .../java/org/photonvision/server/Server.java | 1 + photon-targeting/build.gradle | 1 + .../common/dataflow/structures/Packet.java | 17 + .../estimation/CameraTargetRelation.java | 29 +- .../photonvision/estimation/OpenCVHelp.java | 30 +- .../estimation/RotTrlTransform3d.java | 29 +- .../photonvision/estimation/TargetModel.java | 29 +- .../estimation/VisionEstimation.java | 30 +- .../targeting/MultiTargetPNPResults.java | 93 ++++ .../photonvision/targeting/PNPResults.java | 170 +++++++ .../targeting/PhotonPipelineResult.java | 49 +- .../targeting/PhotonTrackedTarget.java | 35 +- .../org/photonvision/utils/PacketUtils.java | 49 ++ .../src/main/include/PhotonCameraWrapper.h | 4 +- .../src/main/java/frc/robot/Vision.java | 3 +- 72 files changed, 1707 insertions(+), 1800 deletions(-) create mode 100644 photon-client/src/components/settings/ApriltagControlCard.vue create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java delete mode 100644 photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java delete mode 100644 photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java create mode 100644 photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp delete mode 100644 photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp create mode 100644 photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h delete mode 100644 photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h delete mode 100644 photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp rename {photon-lib => photon-targeting}/src/main/java/org/photonvision/estimation/CameraTargetRelation.java (62%) rename {photon-lib => photon-targeting}/src/main/java/org/photonvision/estimation/OpenCVHelp.java (95%) rename {photon-lib => photon-targeting}/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java (86%) rename {photon-lib => photon-targeting}/src/main/java/org/photonvision/estimation/TargetModel.java (86%) rename {photon-lib => photon-targeting}/src/main/java/org/photonvision/estimation/VisionEstimation.java (81%) create mode 100644 photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java create mode 100644 photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java create mode 100644 photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java diff --git a/.gitignore b/.gitignore index 2bb8de0059..609770cdf5 100644 --- a/.gitignore +++ b/.gitignore @@ -114,7 +114,6 @@ fabric.properties **/.settings **/.classpath **/.project -**/settings **/dependency-reduced-pom.xml # photon-server/photon-vision.iml diff --git a/photon-client/src/App.vue b/photon-client/src/App.vue index bcd3e27e94..80711960f1 100644 --- a/photon-client/src/App.vue +++ b/photon-client/src/App.vue @@ -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); diff --git a/photon-client/src/components/app/photon-log-view.vue b/photon-client/src/components/app/photon-log-view.vue index e5442bc28c..103ea94733 100644 --- a/photon-client/src/components/app/photon-log-view.vue +++ b/photon-client/src/components/app/photon-log-view.vue @@ -71,7 +71,7 @@ document.addEventListener("keydown", (e) => { - There are no Logs to show + There are no logs to show diff --git a/photon-client/src/components/settings/ApriltagControlCard.vue b/photon-client/src/components/settings/ApriltagControlCard.vue new file mode 100644 index 0000000000..033dbd5474 --- /dev/null +++ b/photon-client/src/components/settings/ApriltagControlCard.vue @@ -0,0 +1,93 @@ + + + + + diff --git a/photon-client/src/components/settings/DeviceControlCard.vue b/photon-client/src/components/settings/DeviceControlCard.vue index b7dfeff249..f18db799bf 100644 --- a/photon-client/src/components/settings/DeviceControlCard.vue +++ b/photon-client/src/components/settings/DeviceControlCard.vue @@ -135,7 +135,8 @@ enum ImportType { AllSettings, HardwareConfig, HardwareSettings, - NetworkConfig + NetworkConfig, + ApriltagFieldLayout } const showImportDialog = ref(false); const importType = ref(-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%" /> diff --git a/photon-client/src/components/settings/MetricsCard.vue b/photon-client/src/components/settings/MetricsCard.vue index bc86036409..081e26cd73 100644 --- a/photon-client/src/components/settings/MetricsCard.vue +++ b/photon-client/src/components/settings/MetricsCard.vue @@ -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." }); } }) diff --git a/photon-client/src/stores/StateStore.ts b/photon-client/src/stores/StateStore.ts index 6c2a9957e8..3f51b56fb2 100644 --- a/photon-client/src/stores/StateStore.ts +++ b/photon-client/src/stores/StateStore.ts @@ -24,7 +24,7 @@ interface StateStore { logMessages: LogMessage[]; currentCameraIndex: number; - pipelineResults?: PipelineResult; + backendResults: Record; 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 = { diff --git a/photon-client/src/stores/settings/CameraSettingsStore.ts b/photon-client/src/stores/settings/CameraSettingsStore.ts index 356d12faae..861b192ec3 100644 --- a/photon-client/src/stores/settings/CameraSettingsStore.ts +++ b/photon-client/src/stores/settings/CameraSettingsStore.ts @@ -173,7 +173,9 @@ export const useCameraSettingsStore = defineStore("cameraSettings", { settings: Partial, 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. diff --git a/photon-client/src/stores/settings/GeneralSettingsStore.ts b/photon-client/src/stores/settings/GeneralSettingsStore.ts index 2e5cd3b89b..5202d8626e 100644 --- a/photon-client/src/stores/settings/GeneralSettingsStore.ts +++ b/photon-client/src/stores/settings/GeneralSettingsStore.ts @@ -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 = { diff --git a/photon-client/src/types/PhotonTrackingTypes.ts b/photon-client/src/types/PhotonTrackingTypes.ts index 0bd3028c4d..ee19240ffa 100644 --- a/photon-client/src/types/PhotonTrackingTypes.ts +++ b/photon-client/src/types/PhotonTrackingTypes.ts @@ -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; } diff --git a/photon-client/src/types/PipelineTypes.ts b/photon-client/src/types/PipelineTypes.ts index 2f2a090316..026235d725 100644 --- a/photon-client/src/types/PipelineTypes.ts +++ b/photon-client/src/types/PipelineTypes.ts @@ -214,6 +214,8 @@ export interface AprilTagPipelineSettings extends PipelineSettings { debug: boolean; threads: number; tagFamily: AprilTagFamily; + doMultiTarget: boolean; + doSingleTargetAlways: boolean; } export type ConfigurableAprilTagPipelineSettings = Partial< Omit @@ -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 diff --git a/photon-client/src/types/WebsocketDataTypes.ts b/photon-client/src/types/WebsocketDataTypes.ts index 0a31efb769..0114e19e79 100644 --- a/photon-client/src/types/WebsocketDataTypes.ts +++ b/photon-client/src/types/WebsocketDataTypes.ts @@ -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; lighting: Required; 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; export interface WebsocketCalibrationData { patternWidth: number; @@ -106,7 +86,7 @@ export interface IncomingWebsocketData { updatePipelineResult?: WebsocketPipelineResultUpdate; networkInfo?: { possibleRios: string[]; - deviceips: string[]; + deviceIps: string[]; }; mutatePipelineSettings?: Partial; cameraIndex?: number; // Sent when mutating pipeline settings to check against currently active diff --git a/photon-client/src/views/GeneralSettingsView.vue b/photon-client/src/views/GeneralSettingsView.vue index 4cc5d0206d..edd3e67edd 100644 --- a/photon-client/src/views/GeneralSettingsView.vue +++ b/photon-client/src/views/GeneralSettingsView.vue @@ -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"; diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java index 48a6e011bf..1d87dd17ab 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java @@ -255,6 +255,10 @@ public boolean saveUploadedNetworkConfig(Path uploadPath) { 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(); diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigProvider.java index a434df0e2b..9964d0ca93 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigProvider.java @@ -39,4 +39,6 @@ public void clearConfig() { public abstract boolean saveUploadedHardwareSettings(Path uploadPath); public abstract boolean saveUploadedNetworkConfig(Path uploadPath); + + public abstract boolean saveUploadedAprilTagFieldLayout(Path uploadPath); } diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java index 6dca0ddcb2..74d5d472fe 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java @@ -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 @@ protected LegacyConfigProvider(Path configDirectoryFile) { 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 @@ public void load() { HardwareConfig hardwareConfig; HardwareSettings hardwareSettings; NetworkConfig networkConfig; + AprilTagFieldLayout atfl = null; if (hardwareConfigFile.exists()) { try { @@ -175,11 +183,38 @@ public void load() { } } + 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 cameraConfigurations = loadCameraConfigs(); this.config = new PhotonConfiguration( - hardwareConfig, hardwareSettings, networkConfig, cameraConfigurations); + hardwareConfig, hardwareSettings, networkConfig, atfl, cameraConfigurations); } @Override @@ -400,6 +435,10 @@ public Path getNetworkConfigFile() { 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 @@ public boolean saveUploadedNetworkConfig(Path uploadPath) { 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(); diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java index 262cdd7111..fac672636c 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java @@ -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.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 cameraConfigurations; + private AprilTagFieldLayout atfl; + private HashMap 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 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 HardwareSettings getHardwareSettings() { 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 Map toHashMap() { lightingConfig.brightness = hardwareSettings.ledBrightnessPercentage; lightingConfig.supported = !hardwareConfig.ledPins.isEmpty(); settingsSubmap.put("lighting", SerializationUtils.objectToHashMap(lightingConfig)); - + // General Settings var generalSubmap = new HashMap(); generalSubmap.put("version", PhotonVersion.versionString); generalSubmap.put( @@ -126,8 +142,17 @@ public Map toHashMap() { 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; } diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java index df63b27588..8a0c755ad8 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java @@ -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 @@ static class TableKeys { 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 void load() { HardwareConfig hardwareConfig; HardwareSettings hardwareSettings; NetworkConfig networkConfig; + AprilTagFieldLayout atfl; try { hardwareConfig = @@ -229,6 +234,25 @@ public void load() { 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 void load() { 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 boolean saveUploadedNetworkConfig(Path uploadPath) { return saveOneFile(TableKeys.NETWORK_CONFIG, uploadPath); } + @Override + public boolean saveUploadedAprilTagFieldLayout(Path uploadPath) { + return saveOneFile(TableKeys.ATFL_CONFIG_FILE, uploadPath); + } + private HashMap loadCameraConfigs(Connection conn) { HashMap loadedConfigurations = new HashMap<>(); diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 56bae44bd7..d985c710f5 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -19,20 +19,15 @@ 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 void updateCameraNickname(String newCameraNickname) { 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 void accept(CVPipelineResult result) { // TODO...nt4... is this needed? rootTable.getInstance().flush(); } - - public static List simpleFromTrackedTargets(List targets) { - var ret = new ArrayList(); - for (var t : targets) { - var minAreaRectCorners = new ArrayList(); - var detectedCorners = new ArrayList(); - { - 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; - } } diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java index 8abb97866b..a62e0f3b4c 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java @@ -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.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 static NetworkTablesManager getInstance() { private static class NTLogger implements Consumer { 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 void accept(NetworkTableEvent event) { } } + 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 @@ private void setClientMode(String ntServerAddress) { 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 @@ private void ntTick() { 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..."); } diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java index 0a9de04448..17ed240921 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java @@ -24,6 +24,7 @@ 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 UIDataPublisher(int index) { public void accept(CVPipelineResult result) { long now = System.currentTimeMillis(); - var dataMap = new HashMap(); - dataMap.put("latency", result.getLatencyMillis()); - // only update the UI at 15hz if (lastUIResultUpdateTime + 1000.0 / 10.0 > now) return; - var uiMap = new HashMap>(); - + var dataMap = new HashMap(); dataMap.put("fps", result.fps); - - var targets = result.targets; - - var uiTargets = new ArrayList>(); - for (var t : targets) { + dataMap.put("latency", result.getLatencyMillis()); + var uiTargets = new ArrayList>(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(); + 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>(); uiMap.put(index, dataMap); DataChangeService.getInstance() diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java index 008724e9b7..2a049cccae 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java @@ -138,6 +138,8 @@ public void setBrightnessPercent(int percent) { private void onJvmExit() { logger.info("Shutting down LEDs..."); if (visionLED != null) visionLED.setState(false); + + logger.info("Force-flushing settings..."); ConfigManager.getInstance().saveToDisk(); } diff --git a/photon-core/src/main/java/org/photonvision/common/networking/RoborioFinder.java b/photon-core/src/main/java/org/photonvision/common/networking/RoborioFinder.java index e6b42112fc..605480e296 100644 --- a/photon-core/src/main/java/org/photonvision/common/networking/RoborioFinder.java +++ b/photon-core/src/main/java/org/photonvision/common/networking/RoborioFinder.java @@ -45,7 +45,7 @@ public void findRios() { // 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()); diff --git a/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java b/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java index a211b545ed..6fc3ea9c30 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java @@ -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 static HashMap objectToHashMap(Object src) { } return ret; } + + public static HashMap transformToHashMap(Transform3d transform) { + var ret = new HashMap(); + 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; + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java index fca4fdf261..15a6d0b9e0 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java @@ -19,7 +19,10 @@ 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 MatOfDouble getAsMatOfDouble() { return this.wrappedMatOfDouble; } + @JsonIgnore + public Matrix getAsWpilibMat() { + if (wpilibMat == null) { + wpilibMat = new Matrix(new SimpleMatrix(rows, cols, true, data)); + } + return (Matrix) wpilibMat; + } + @Override public void release() { getAsMat().release(); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java index 32ea2938b7..5c1497db3e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java @@ -27,8 +27,6 @@ public class AprilTagDetectionPipe extends CVPipe, AprilTagDetectionPipeParams> { private final AprilTagDetector m_detector = new AprilTagDetector(); - boolean useNativePoseEst; - public AprilTagDetectionPipe() { super(); @@ -62,8 +60,4 @@ public void setParams(AprilTagDetectionPipeParams newParams) { super.setParams(newParams); } - - public void setNativePoseEstimationEnabled(boolean enabled) { - this.useNativePoseEst = enabled; - } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java index 9738202e3e..3c6563e182 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java @@ -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 void setParams(AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams super.setParams(newParams); } - public void setNativePoseEstimationEnabled(boolean enabled) { - this.useNativePoseEst = enabled; - } - public static class AprilTagPoseEstimatorPipeParams { final AprilTagPoseEstimator.Config config; final CameraCalibrationCoefficients calibration; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java new file mode 100644 index 0000000000..5ce9f124cc --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java @@ -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 . + */ + +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, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> { + private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule); + + private boolean hasWarned = false; + + @Override + protected MultiTargetPNPResults process(List 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 targetList) { + // Find tag IDs that exist in the tag layout + var tagIDsUsed = new ArrayList(); + 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; + } + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index c7ffe1a83c..d1837ff770 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -21,11 +21,16 @@ 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.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 { 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 @@ protected void setPipeParamsImpl() { 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 @@ protected void setPipeParamsImpl() { protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings settings) { long sumPipeNanosElapsed = 0L; - List 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> tagDetectionPipeResult; tagDetectionPipeResult = aprilTagDetectionPipe.run(frame.processedImage); sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed; - targetList = new ArrayList<>(); - for (AprilTagDetection detection : tagDetectionPipeResult.output) { + List detections = tagDetectionPipeResult.output; + List usedDetections = new ArrayList<>(); + List 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()); + targetList.add(target); + } - target.setBestCameraToTarget3d( - new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation())); - target.setAltCameraToTarget3d( - new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation())); + // 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; + } - targetList.add(target); + // 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); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java index 4821c7b759..47faf82e29 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java @@ -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 AprilTagPipelineSettings() { } @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; } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java index 261b8fd522..95008e77f9 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java @@ -34,6 +34,7 @@ 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 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se sumPipeNanosElapsed, fps, // Unused but here in case Collections.emptyList(), + new MultiTargetPNPResults(), new Frame( new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties)); } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java index aa9fd800ab..5993cc9531 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java @@ -20,6 +20,7 @@ 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 targets; public final Frame inputAndOutputFrame; + public MultiTargetPNPResults multiTagResult; public CVPipelineResult( double processingNanos, double fps, List targets, Frame inputFrame) { + this(processingNanos, fps, targets, new MultiTargetPNPResults(), inputFrame); + } + + public CVPipelineResult( + double processingNanos, + double fps, + List 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 targets) { - this(processingNanos, fps, targets, null); + public CVPipelineResult( + double processingNanos, + double fps, + List targets, + MultiTargetPNPResults multiTagResults) { + this(processingNanos, fps, targets, multiTagResults, null); } public boolean hasTargets() { diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java index 922ec5e6ec..9be1e9162c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -22,10 +22,19 @@ 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 TrackedTarget( 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 TrackedTarget( 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 HashMap toHashMap() { 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 transformToMap(Transform3d transform) { - var ret = new HashMap(); - 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 simpleFromTrackedTargets(List targets) { + var ret = new ArrayList(); + for (var t : targets) { + var minAreaRectCorners = new ArrayList(); + var detectedCorners = new ArrayList(); + { + 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; diff --git a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java index 7d9f9b6dcd..a355ab00f3 100644 --- a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java +++ b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java @@ -26,6 +26,7 @@ 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 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 targetsUsed) { + Pose3d estimatedPose, + double timestampSeconds, + List targetsUsed, + PoseStrategy strategy) { this.estimatedPose = estimatedPose; this.timestampSeconds = timestampSeconds; this.targetsUsed = targetsUsed; + this.strategy = strategy; } } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index b9978a8021..9500629ca3 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -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.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 void close() { pipelineIndexRequest.close(); cameraIntrinsicsSubscriber.close(); cameraDistortionSubscriber.close(); + atflPublisher.close(); } private final String path; @@ -114,6 +121,7 @@ public static void setVersionCheckEnabled(boolean enabled) { 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 PhotonCamera(NetworkTableInstance instance, String cameraName) { 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 boolean isConnected() { 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> getCameraMatrix() { var cameraMatrix = cameraIntrinsicsSubscriber.get(); if (cameraMatrix != null && cameraMatrix.length == 9) { diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 0eecc02880..ed29a9c265 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -68,8 +68,17 @@ public enum PoseStrategy { /** 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 void setPrimaryStrategy(PoseStrategy strategy) { */ 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 @@ private Optional update( 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 @@ private Optional update( return estimatedPose; } - private Optional multiTagPNPStrategy( + private Optional multiTagOnCoprocStrategy( + PhotonPipelineResult result, + Optional> cameraMatrixOpt, + Optional> 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 multiTagOnRioStrategy( PhotonPipelineResult result, Optional> cameraMatrixOpt, Optional> distCoeffsOpt) { @@ -395,7 +429,11 @@ private Optional multiTagPNPStrategy( .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 @@ private Optional lowestAmbiguityStrategy(PhotonPipelineResul .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), result.getTimestampSeconds(), - result.getTargets())); + result.getTargets(), + PoseStrategy.LOWEST_AMBIGUITY)); } /** @@ -494,7 +533,8 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin .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 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin .transformBy(target.getBestCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), result.getTimestampSeconds(), - result.getTargets()); + result.getTargets(), + PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); } } @@ -568,13 +609,19 @@ private Optional closestToReferencePoseStrategy( 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 @@ private Optional averageBestTargetsStrategy(PhotonPipelineRe .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 @@ private Optional averageBestTargetsStrategy(PhotonPipelineRe 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)); } /** diff --git a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java deleted file mode 100644 index 4dd6dcdaaa..0000000000 --- a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java +++ /dev/null @@ -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 { - /** - * - * - *
    - *
  • LOWEST_AMBIGUITY: Choose the Pose with the lowest ambiguity - *
  • CLOSEST_TO_CAMERA_HEIGHT: Choose the Pose which is closest to the camera - * height - *
  • CLOSEST_TO_REFERENCE_POSE: Choose the Pose which is closest to the pose - * from setReferencePose() - *
  • CLOSEST_TO_LAST_POSE: Choose the Pose which is closest to the last pose - * calculated - *
- */ - 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> cameras; - private Pose3d lastPose; - - private Pose3d referencePose; - private Set 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> 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> update() { - if (cameras.isEmpty()) { - DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false); - return Optional.empty(); - } - - Pair pair = getResultFromActiveStrategy(); - - if (pair != null) { - lastPose = pair.getFirst(); - } - - return Optional.ofNullable(pair); - } - - private Pair 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 lowestAmbiguityStrategy() { - int lowestAI = -1; - int lowestAJ = -1; - double lowestAmbiguityScore = 10; - ArrayList results = new ArrayList(cameras.size()); - - // Sample result from each camera - for (int i = 0; i < cameras.size(); i++) { - Pair 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 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 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 closestToCameraHeightStrategy() { - double smallestHeightDifference = Double.MAX_VALUE; - double latency = 0; - Pose3d pose = null; - - for (int i = 0; i < cameras.size(); i++) { - Pair p = cameras.get(i); - var result = p.getFirst().getLatestResult(); - List targets = result.targets; - for (int j = 0; j < targets.size(); j++) { - PhotonTrackedTarget target = targets.get(j); - Optional 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 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 p = cameras.get(i); - var result = p.getFirst().getLatestResult(); - List targets = result.targets; - for (int j = 0; j < targets.size(); j++) { - PhotonTrackedTarget target = targets.get(j); - Optional 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 closestToLastPoseStrategy() { - setReferencePose(lastPose); - return closestToReferencePoseStrategy(); - } - - /** Return the average of the best target poses using ambiguity as weight */ - private Pair averageBestTargetsStrategy() { - // Pair of Double, Double = Ambiguity, Mili - List>> tempPoses = new ArrayList<>(); - double totalAmbiguity = 0; - for (int i = 0; i < cameras.size(); i++) { - Pair p = cameras.get(i); - var result = p.getFirst().getLatestResult(); - List targets = result.targets; - for (int j = 0; j < targets.size(); j++) { - PhotonTrackedTarget target = targets.get(j); - Optional 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> 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); - } - } -} diff --git a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java deleted file mode 100644 index 89adf94bcf..0000000000 --- a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java +++ /dev/null @@ -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). - * - *

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; - } -} diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index e50588c640..1c9ef2ce1e 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -50,9 +50,9 @@ 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; diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java index 9f301486a6..58e3e2fcf7 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java @@ -35,6 +35,7 @@ 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 void submitProcessedFrame( 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()); diff --git a/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp new file mode 100644 index 0000000000..91d6ffd6c3 --- /dev/null +++ b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp @@ -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(target.fiducialIdsUsed[i]); + } else { + packet << static_cast(-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 diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp index 9f6d0a8218..462dd25a90 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp @@ -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(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(); diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index f25e01eba5..080243749f 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -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 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 PhotonPoseEstimator::LowestAmbiguityStrategy( fiducialPose.value() .TransformBy(bestTarget.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), - result.GetTimestamp(), result.GetTargets()}; + result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY}; } std::optional @@ -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> detail::CalcTagCorners( @@ -351,7 +357,24 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { Rotation3d(rv)); } -std::optional PhotonPoseEstimator::MultiTagPnpStrategy( +std::optional PhotonPoseEstimator::MultiTagOnCoprocStrategy( + PhotonPipelineResult result, std::optional camMat, + std::optional 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 PhotonPoseEstimator::MultiTagOnRioStrategy( PhotonPipelineResult result, std::optional camMat, std::optional distCoeffs) { using namespace frc; @@ -404,7 +427,7 @@ std::optional PhotonPoseEstimator::MultiTagPnpStrategy( return photonlib::EstimatedRobotPose( pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(), - result.GetTargets()); + result.GetTargets(), MULTI_TAG_PNP_ON_RIO); } std::optional @@ -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 diff --git a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp deleted file mode 100644 index 4d25a0a482..0000000000 --- a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "photonlib/PhotonCamera.h" -#include "photonlib/PhotonPipelineResult.h" -#include "photonlib/PhotonTrackedTarget.h" - -namespace photonlib { -RobotPoseEstimator::RobotPoseEstimator( - std::shared_ptr tags, PoseStrategy strat, - std::vector, frc::Transform3d>> - cams) - : aprilTags(tags), - strategy(strat), - cameras(std::move(cams)), - lastPose(frc::Pose3d()), - referencePose(frc::Pose3d()) {} - -std::pair RobotPoseEstimator::Update() { - if (cameras.empty()) { - return std::make_pair(lastPose, units::second_t(0)); - } - - std::pair 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 -RobotPoseEstimator::LowestAmbiguityStrategy() { - int lowestAI = -1; - int lowestAJ = -1; - double lowestAmbiguityScore = std::numeric_limits::infinity(); - for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) { - std::pair, frc::Transform3d> p = cameras[i]; - std::span 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 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 -RobotPoseEstimator::ClosestToCameraHeightStrategy() { - units::meter_t smallestHeightDifference = - units::meter_t(std::numeric_limits::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, frc::Transform3d> p = cameras[i]; - std::span targets = - p.first->GetLatestResult().GetTargets(); - for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) { - PhotonTrackedTarget target = targets[j]; - std::optional 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 -RobotPoseEstimator::ClosestToReferencePoseStrategy() { - units::meter_t smallestDifference = - units::meter_t(std::numeric_limits::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, frc::Transform3d> p = cameras[i]; - std::span targets = - p.first->GetLatestResult().GetTargets(); - for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) { - PhotonTrackedTarget target = targets[j]; - std::optional 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 -RobotPoseEstimator::AverageBestTargetsStrategy() { - std::vector>> - tempPoses; - double totalAmbiguity = 0; - units::second_t timstampSum = units::second_t(0); - - for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) { - std::pair, frc::Transform3d> p = cameras[i]; - std::span 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 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>& 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 diff --git a/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h new file mode 100644 index 0000000000..96681a0307 --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h @@ -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 +#include + +#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 fiducialIdsUsed; + + friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result); + friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result); +}; + +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h index e6f6c4d49b..8370702b3b 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h @@ -31,6 +31,7 @@ #include #include +#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 targets; + MultiTargetPnpResult m_pnpResults; inline static bool HAS_WARNED = false; }; } // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h index 21f6a343cb..7c93ac9ce9 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h @@ -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 targetsUsed; + /** The strategy actually used to produce this pose */ + PoseStrategy strategy; + EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_, - std::span targets) + std::span 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 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 MultiTagOnCoprocStrategy( + PhotonPipelineResult result, std::optional camMat, + std::optional 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 MultiTagPnpStrategy( + std::optional MultiTagOnRioStrategy( PhotonPipelineResult result, std::optional camMat, std::optional distCoeffs); diff --git a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h deleted file mode 100644 index c9282b5920..0000000000 --- a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h +++ /dev/null @@ -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 -#include -#include -#include - -#include -#include - -#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, frc::Transform3d>; - using size_type = std::vector::size_type; - - /** - * Create a new RobotPoseEstimator. - * - *

Example: {@code

Map map = new HashMap<>(); - *

map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is - * at (1.0,2.0,3.0) } - * - * @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 aprilTags, - PoseStrategy strategy, std::vector cameras); - - /** - * Get the AprilTagFieldLayout being used by the PositionEstimator. - * - * @return the AprilTagFieldLayout - */ - std::shared_ptr getFieldLayout() const { - return aprilTags; - } - - /** - * Set the cameras to be used by the PoseEstimator. - * - * @param cameras cameras to set. - */ - inline void SetCameras( - const std::vector, - 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 Update(); - - private: - std::shared_ptr aprilTags; - PoseStrategy strategy; - std::vector 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 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 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 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 AverageBestTargetsStrategy(); -}; - -} // namespace photonlib diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java index 4f84afb720..c29a629b5b 100644 --- a/photon-lib/src/test/java/org/photonvision/PacketTest.java +++ b/photon-lib/src/test/java/org/photonvision/PacketTest.java @@ -30,6 +30,8 @@ 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 @@ void testSimplePipelineResult() { 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); + } } diff --git a/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java b/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java index 5ead3be618..fa70467c5f 100644 --- a/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java +++ b/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java @@ -84,7 +84,10 @@ public void testMeme() throws IOException, InterruptedException { 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); diff --git a/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp deleted file mode 100644 index 09a2bdf461..0000000000 --- a/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "gtest/gtest.h" -#include "photonlib/PhotonCamera.h" -#include "photonlib/PhotonPipelineResult.h" -#include "photonlib/PhotonTrackedTarget.h" -#include "photonlib/RobotPoseEstimator.h" - -static wpi::SmallVector, 4> corners{ - std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}; -static std::vector> detectedCorners{ - std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}; - -TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) { - std::vector 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 aprilTags = - std::make_shared(tags, 54_ft, 27_ft); - - std::vector< - std::pair, frc::Transform3d>> - cameras; - - std::shared_ptr cameraOne = - std::make_shared("test"); - std::shared_ptr cameraTwo = - std::make_shared("test"); - - wpi::SmallVector 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 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 estimatedPose = - estimator.Update(); - frc::Pose3d pose = estimatedPose.first; - - EXPECT_NEAR(11, units::unit_cast(estimatedPose.second) / 1000.0, .01); - EXPECT_NEAR(1, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(3, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(2, units::unit_cast(pose.Z()), .01); -} - -TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) { - std::vector 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 aprilTags = - std::make_shared(tags, 54_ft, 27_ft); - - std::vector< - std::pair, frc::Transform3d>> - cameras; - - std::shared_ptr cameraOne = - std::make_shared("test"); - std::shared_ptr cameraTwo = - std::make_shared("test"); - - wpi::SmallVector 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 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 estimatedPose = - estimator.Update(); - frc::Pose3d pose = estimatedPose.first; - - EXPECT_NEAR(12, units::unit_cast(estimatedPose.second) / 1000.0, .01); - EXPECT_NEAR(4, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(4, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(4, units::unit_cast(pose.Z()), .01); -} - -TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) { - std::vector 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 aprilTags = - std::make_shared(tags, 54_ft, 27_ft); - - std::vector< - std::pair, frc::Transform3d>> - cameras; - - std::shared_ptr cameraOne = - std::make_shared("test"); - std::shared_ptr cameraTwo = - std::make_shared("test"); - - wpi::SmallVector 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 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 estimatedPose = - estimator.Update(); - frc::Pose3d pose = estimatedPose.first; - - EXPECT_NEAR(17, units::unit_cast(estimatedPose.second) / 1000.0, .01); - EXPECT_NEAR(1, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(1.1, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(.9, units::unit_cast(pose.Z()), .01); -} - -TEST(RobotPoseEstimatorTest, ClosestToLastPose) { - std::vector 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 aprilTags = - std::make_shared(tags, 54_ft, 27_ft); - - std::vector< - std::pair, frc::Transform3d>> - cameras; - - std::shared_ptr cameraOne = - std::make_shared("test"); - std::shared_ptr cameraTwo = - std::make_shared("test"); - - wpi::SmallVector 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 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 estimatedPose = - estimator.Update(); - frc::Pose3d pose = estimatedPose.first; - - wpi::SmallVector 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 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, 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(estimatedPose.second) / 1000.0, - .01); - EXPECT_NEAR(.9, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(1.1, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(1, units::unit_cast(pose.Z()), .01); -} - -TEST(RobotPoseEstimatorTest, AverageBestPoses) { - std::vector 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 aprilTags = - std::make_shared(tags, 54_ft, 27_ft); - - std::vector< - std::pair, frc::Transform3d>> - cameras; - - std::shared_ptr cameraOne = - std::make_shared("test"); - std::shared_ptr cameraTwo = - std::make_shared("test"); - - wpi::SmallVector 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 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 estimatedPose = - estimator.Update(); - frc::Pose3d pose = estimatedPose.first; - - EXPECT_NEAR(15.0, units::unit_cast(estimatedPose.second) / 1000.0, - .01); - EXPECT_NEAR(2.15, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(2.15, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(2.15, units::unit_cast(pose.Z()), .01); -} diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java index c255748195..69726c9adb 100644 --- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java @@ -246,6 +246,48 @@ public static void onNetworkConfigRequest(Context ctx) { } } + 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"); diff --git a/photon-server/src/main/java/org/photonvision/server/Server.java b/photon-server/src/main/java/org/photonvision/server/Server.java index 724459e14f..b6595d5b94 100644 --- a/photon-server/src/main/java/org/photonvision/server/Server.java +++ b/photon-server/src/main/java/org/photonvision/server/Server.java @@ -97,6 +97,7 @@ public static void start(int port) { 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); diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 168ecc981e..e4cc2d7d96 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -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 diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java index 02a8f0774d..e97e3f1658 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java @@ -85,6 +85,16 @@ public void encode(byte src) { 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 double[] decodeDoubleArray(int len) { } return ret; } + + public short decodeShort() { + if (packetData.length < readPos + 1) { + return 0; + } + return (short) ((0xff & packetData[readPos++]) << 8 | (0xff & packetData[readPos++])); + } } diff --git a/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java b/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java similarity index 62% rename from photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java rename to photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java index 40232aa8b8..b0b915c93e 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java @@ -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 . */ package org.photonvision.estimation; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java similarity index 95% rename from photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java rename to photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 67002101ce..3f89918d62 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -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 . */ package org.photonvision.estimation; @@ -52,6 +45,7 @@ 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 { diff --git a/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java similarity index 86% rename from photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java rename to photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java index 0fac0334eb..2b79e920d6 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java @@ -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 . */ package org.photonvision.estimation; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java similarity index 86% rename from photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java rename to photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java index 90044abb38..4a4c22449d 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java @@ -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 . */ package org.photonvision.estimation; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java similarity index 81% rename from photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java rename to photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index 3c9c2c152c..387176d57b 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -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 . */ package org.photonvision.estimation; @@ -36,6 +29,7 @@ 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; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java new file mode 100644 index 0000000000..453c0a57e5 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java @@ -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 . + */ + +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 fiducialIDsUsed = List.of(); + + public MultiTargetPNPResults() {} + + public MultiTargetPNPResults(PNPResults results, List ids) { + estimatedPose = results; + fiducialIDsUsed = ids; + } + + public static MultiTargetPNPResults createFromPacket(Packet packet) { + var results = PNPResults.createFromPacket(packet); + var ids = new ArrayList(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 + + "]"; + } +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java new file mode 100644 index 0000000000..11a77547fc --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java @@ -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 . + */ + +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). + * + *

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 + + "]"; + } +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index f2a5292454..4472078f8a 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -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 PhotonPipelineResult(double latencyMillis, List targ 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 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 List getTargets() { 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 List getTargets() { 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 Packet createFromPacket(Packet packet) { 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 int hashCode() { 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 boolean equals(Object obj) { 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 + + "]"; + } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index a15fca87d5..40263f5cc5 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -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 boolean equals(Object obj) { 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 list) { packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE)); for (int i = 0; i < list.size(); i++) { @@ -253,8 +228,8 @@ public Packet createFromPacket(Packet packet) { 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 Packet populatePacket(Packet packet) { 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++) { diff --git a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java new file mode 100644 index 0000000000..43a0dd4cf6 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java @@ -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 . + */ + +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()); + } +} diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h index 5caab2ad53..c96ff8078d 100644 --- a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h +++ b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h @@ -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 Update( frc::Pose2d estimatedPose) { diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java index 1d9b2ab9ed..d1e88c9258 100644 --- a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java +++ b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java @@ -56,7 +56,8 @@ public 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