From 2599cd6e13909bafd425cccf8dcc923c43ba64f2 Mon Sep 17 00:00:00 2001 From: Ryan Shoff Date: Fri, 8 Nov 2024 16:35:49 -0600 Subject: [PATCH 01/12] tested multitag --- .../dashboard/tabs/AprilTagCudaTab.vue | 93 +++++++ .../org/photonvision/jni/GpuDetectorJNI.java | 22 ++ .../pipe/impl/AprilTagDetectionCudaPipe.java | 99 ++++++++ .../impl/AprilTagDetectionCudaPipeParams.java | 58 +++++ .../vision/pipeline/AprilTagCudaPipeline.java | 235 ++++++++++++++++++ .../AprilTagCudaPipelineSettings.java | 89 +++++++ 6 files changed, 596 insertions(+) create mode 100644 photon-client/src/components/dashboard/tabs/AprilTagCudaTab.vue create mode 100644 photon-core/src/main/java/org/photonvision/jni/GpuDetectorJNI.java create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionCudaPipe.java create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionCudaPipeParams.java create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagCudaPipeline.java create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagCudaPipelineSettings.java diff --git a/photon-client/src/components/dashboard/tabs/AprilTagCudaTab.vue b/photon-client/src/components/dashboard/tabs/AprilTagCudaTab.vue new file mode 100644 index 0000000000..4c4465bddc --- /dev/null +++ b/photon-client/src/components/dashboard/tabs/AprilTagCudaTab.vue @@ -0,0 +1,93 @@ + + + diff --git a/photon-core/src/main/java/org/photonvision/jni/GpuDetectorJNI.java b/photon-core/src/main/java/org/photonvision/jni/GpuDetectorJNI.java new file mode 100644 index 0000000000..086596b7ab --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/jni/GpuDetectorJNI.java @@ -0,0 +1,22 @@ +package org.photonvision.jni; + +import edu.wpi.first.apriltag.AprilTagDetection; +import java.io.IOException; + +public class GpuDetectorJNI { + static boolean libraryLoaded = false; + + static { + if(!libraryLoaded) + System.loadLibrary("971apriltag"); + libraryLoaded = true; + } + + public static native long createGpuDetector(int width, int height); + public static native void destroyGpuDetector(long handle); + public static native void setparams(long handle, double fx, double cx, + double fy, double cy, double k1, double k2, double p1, + double p2, double k3); + public static native AprilTagDetection[] processimage(long handle, long p); + +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionCudaPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionCudaPipe.java new file mode 100644 index 0000000000..5521fbe70a --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionCudaPipe.java @@ -0,0 +1,99 @@ +/* + * 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.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagDetector; +import java.util.List; +import org.photonvision.vision.opencv.CVMat; +import org.photonvision.vision.opencv.Releasable; +import org.photonvision.vision.pipe.CVPipe; +import org.photonvision.jni.GpuDetectorJNI; +import org.opencv.core.Mat; + +public class AprilTagDetectionCudaPipe + extends CVPipe, AprilTagDetectionCudaPipeParams> + implements Releasable { + //private AprilTagDetector m_detector = new AprilTagDetector(); + private GpuDetectorJNI m_cudadetector = new GpuDetectorJNI(); + private long handle = 0; + + public AprilTagDetectionCudaPipe() { + super(); + + //m_detector.addFamily("tag16h5"); + //m_detector.addFamily("tag36h11"); + handle = m_cudadetector.createGpuDetector(640,480); // just a guess + } + + @Override + protected List process(CVMat in) { + if (in.getMat().empty()) { + return List.of(); + } + + //if (m_detector == null) { + // throw new RuntimeException("Apriltag detector was released!"); + //} + + var ret = m_cudadetector.processimage(handle, in.getMat().getNativeObjAddr()); + + if (ret == null) { + return List.of(); + } + + return List.of(ret); + } + + @Override + public void setParams(AprilTagDetectionCudaPipeParams newParams) { + if (this.params == null || !this.params.equals(newParams)) { + //m_detector.setConfig(newParams.detectorParams); + + //m_detector.clearFamilies(); + //m_detector.addFamily(newParams.family.getNativeName()); + + if( newParams.cameraCalibrationCoefficients == null ) return; + + final Mat cameraMatrix = newParams.cameraCalibrationCoefficients.getCameraIntrinsicsMat(); + final Mat distCoeffs = newParams.cameraCalibrationCoefficients.getDistCoeffsMat(); + if(cameraMatrix == null || distCoeffs == null) return; + 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]; + var k1 = distCoeffs.get(0, 0)[0]; + var k2 = distCoeffs.get(0, 1)[0]; + var k3 = distCoeffs.get(0, 4)[0]; + var p1 = distCoeffs.get(0, 2)[0]; + var p2 = distCoeffs.get(0, 3)[0]; + + m_cudadetector.setparams(handle,fx,cx,fy,cy,k1,k2,p1,p2,k3); + } + + super.setParams(newParams); + } + + @Override + public void release() { + //m_detector.close(); + //m_detector = null; + m_cudadetector.destroyGpuDetector(handle); + m_cudadetector = null; + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionCudaPipeParams.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionCudaPipeParams.java new file mode 100644 index 0000000000..fe04d24fd6 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionCudaPipeParams.java @@ -0,0 +1,58 @@ +/* + * 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.AprilTagDetector; +import org.photonvision.vision.apriltag.AprilTagFamily; +import org.photonvision.vision.calibration.CameraCalibrationCoefficients; + + +public class AprilTagDetectionCudaPipeParams { + public final AprilTagFamily family; + public final AprilTagDetector.Config detectorParams; + public final CameraCalibrationCoefficients cameraCalibrationCoefficients; + + public AprilTagDetectionCudaPipeParams(AprilTagFamily tagFamily, AprilTagDetector.Config config, CameraCalibrationCoefficients cal) { + this.family = tagFamily; + this.detectorParams = config; + this.cameraCalibrationCoefficients = cal; + } + + @Override + public int hashCode() { + final int prime = 31; + int result = 1; + result = prime * result + ((family == null) ? 0 : family.hashCode()); + result = prime * result + ((cameraCalibrationCoefficients == null) ? 0 : cameraCalibrationCoefficients.hashCode()); + result = prime * result + ((detectorParams == null) ? 0 : detectorParams.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; + AprilTagDetectionCudaPipeParams other = (AprilTagDetectionCudaPipeParams) obj; + if (family != other.family) return false; + if (cameraCalibrationCoefficients != other.cameraCalibrationCoefficients) return false; + if (detectorParams == null) { + return other.detectorParams == null; + } else return detectorParams.equals(other.detectorParams); + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagCudaPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagCudaPipeline.java new file mode 100644 index 0000000000..ef375edd01 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagCudaPipeline.java @@ -0,0 +1,235 @@ +/* + * 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.pipeline; + +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.apriltag.AprilTagPoseEstimate; +import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; +import edu.wpi.first.math.geometry.CoordinateSystem; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import org.photonvision.common.configuration.ConfigManager; +import org.photonvision.common.util.math.MathUtils; +import org.photonvision.estimation.TargetModel; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.vision.apriltag.AprilTagFamily; +import org.photonvision.vision.frame.Frame; +import org.photonvision.vision.frame.FrameThresholdType; +import org.photonvision.vision.pipe.CVPipe.CVPipeResult; +import org.photonvision.vision.pipe.impl.AprilTagDetectionPipe; +import org.photonvision.vision.pipe.impl.AprilTagDetectionCudaPipe; +import org.photonvision.vision.pipe.impl.AprilTagDetectionCudaPipeParams; +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 AprilTagCudaPipeline extends CVPipeline { + private final AprilTagDetectionCudaPipe aprilTagDetectionPipe = new AprilTagDetectionCudaPipe(); + 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; + + public AprilTagCudaPipeline() { + super(PROCESSING_TYPE); + settings = new AprilTagCudaPipelineSettings(); + } + + public AprilTagCudaPipeline(AprilTagCudaPipelineSettings settings) { + super(PROCESSING_TYPE); + this.settings = settings; + } + + @Override + protected void setPipeParamsImpl() { + // Sanitize thread count - not supported to have fewer than 1 threads + settings.threads = Math.max(1, settings.threads); + + // for now, hard code tag width based on enum value + // 2023/other: best guess is 6in + double tagWidth = Units.inchesToMeters(6); + TargetModel tagModel = TargetModel.kAprilTag16h5; + if (settings.tagFamily == AprilTagFamily.kTag36h11) { + // 2024 tag, 6.5in + tagWidth = Units.inchesToMeters(6.5); + tagModel = TargetModel.kAprilTag36h11; + } + + var config = new AprilTagDetector.Config(); + config.numThreads = settings.threads; + config.refineEdges = settings.refineEdges; + config.quadSigma = (float) settings.blur; + config.quadDecimate = settings.decimate; + aprilTagDetectionPipe.setParams(new AprilTagDetectionCudaPipeParams(settings.tagFamily, config, frameStaticProperties.cameraCalibration)); + + if (frameStaticProperties.cameraCalibration != null) { + var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat(); + 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]; + + 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, tagModel)); + } + } + } + + @Override + protected CVPipelineResult process(Frame frame, AprilTagCudaPipelineSettings settings) { + long sumPipeNanosElapsed = 0L; + + if (frame.type != FrameThresholdType.GREYSCALE) { + // We asked for a GREYSCALE frame, but didn't get one -- best we can do is give up + return new CVPipelineResult(frame.sequenceID, 0, 0, List.of(), frame); + } + + CVPipeResult> tagDetectionPipeResult; + tagDetectionPipeResult = aprilTagDetectionPipe.run(frame.processedImage); + sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed; + + 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; + + usedDetections.add(detection); + + // 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, + null, + new TargetCalculationParameters( + false, null, null, null, null, frameStaticProperties)); + + targetList.add(target); + } + + // Do multi-tag pose estimation + Optional multiTagResult = Optional.empty(); + if (settings.solvePNPEnabled && settings.doMultiTarget) { + var multiTagOutput = multiTagPNPPipe.run(targetList); + sumPipeNanosElapsed += multiTagOutput.nanosElapsed; + multiTagResult = multiTagOutput.output; + } + + // Do single-tag pose estimation + if (settings.solvePNPEnabled) { + // Clear target list that was used for multitag so we can add target transforms + targetList.clear(); + // TODO global state again ew + var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout(); + + for (AprilTagDetection detection : usedDetections) { + AprilTagPoseEstimate tagPoseEstimate = null; + // Do single-tag estimation when "always enabled" or if a tag was not used for multitag + if (settings.doSingleTargetAlways + || !(multiTagResult.isPresent() + && multiTagResult.get().fiducialIDsUsed.contains((short) 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 && multiTagResult.isPresent()) { + // 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.get().estimatedPose.best), tagPose.get()); + // match expected AprilTag coordinate system + camToTag = + CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN()); + // (AprilTag expects Z axis going into tag) + camToTag = + new Transform3d( + camToTag.getTranslation(), + new Rotation3d(0, Math.PI, 0).plus(camToTag.getRotation())); + 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( + frame.sequenceID, sumPipeNanosElapsed, fps, targetList, multiTagResult, frame); + } + + @Override + public void release() { + aprilTagDetectionPipe.release(); + singleTagPoseEstimatorPipe.release(); + super.release(); + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagCudaPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagCudaPipelineSettings.java new file mode 100644 index 0000000000..e1bea6a0ed --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagCudaPipelineSettings.java @@ -0,0 +1,89 @@ +/* + * 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.pipeline; + +import com.fasterxml.jackson.annotation.JsonTypeName; +import org.photonvision.vision.apriltag.AprilTagFamily; +import org.photonvision.vision.target.TargetModel; + +@JsonTypeName("AprilTagCudaPipelineSettings") +public class AprilTagCudaPipelineSettings extends AdvancedPipelineSettings { + public AprilTagFamily tagFamily = AprilTagFamily.kTag36h11; + public int decimate = 1; + public double blur = 0; + public int threads = 4; // Multiple threads seems to be better performance on most platforms + public boolean debug = false; + public boolean refineEdges = true; + public int numIterations = 40; + public int hammingDist = 0; + public int decisionMargin = 35; + public boolean doMultiTarget = false; + public boolean doSingleTargetAlways = false; + + // 3d settings + + public AprilTagCudaPipelineSettings() { + super(); + pipelineType = PipelineType.AprilTagCuda; + outputShowMultipleTargets = true; + targetModel = TargetModel.kAprilTag6p5in_36h11; + cameraExposureRaw = 20; + cameraAutoExposure = false; + ledMode = false; + } + + @Override + 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; + AprilTagCudaPipelineSettings other = (AprilTagCudaPipelineSettings) 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; + } +} From b7dc63db174056692e4d3a97a0930de88a6a0e1f Mon Sep 17 00:00:00 2001 From: Ryan Shoff Date: Fri, 8 Nov 2024 16:36:10 -0600 Subject: [PATCH 02/12] tested multitag --- .../dashboard/CameraAndPipelineSelectCard.vue | 5 ++ .../components/dashboard/ConfigOptions.vue | 17 +++++-- .../components/dashboard/tabs/OutputTab.vue | 3 ++ .../components/dashboard/tabs/TargetsTab.vue | 5 ++ photon-client/src/types/PipelineTypes.ts | 49 +++++++++++++++++-- photon-client/src/types/WebsocketDataTypes.ts | 3 +- .../vision/camera/QuirkyCamera.java | 3 ++ .../USBCameras/GenericUSBCameraSettables.java | 11 +++-- .../camera/USBCameras/USBCameraSource.java | 2 +- .../vision/pipeline/CVPipelineSettings.java | 3 +- .../vision/pipeline/OutputStreamPipeline.java | 4 +- .../vision/pipeline/PipelineType.java | 3 +- .../vision/processes/PipelineManager.java | 12 ++++- .../src/main/resources/web/index.html | 15 +++++- 14 files changed, 116 insertions(+), 19 deletions(-) diff --git a/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue b/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue index 8f5f3b33e3..0229866e21 100644 --- a/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue +++ b/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue @@ -130,6 +130,7 @@ const validNewPipelineTypes = computed(() => { { name: "Reflective", value: WebsocketPipelineType.Reflective }, { name: "Colored Shape", value: WebsocketPipelineType.ColoredShape }, { name: "AprilTag", value: WebsocketPipelineType.AprilTag }, + { name: "AprilTagCuda", value: WebsocketPipelineType.AprilTagCuda }, { name: "Aruco", value: WebsocketPipelineType.Aruco } ]; if (useSettingsStore().general.supportedBackends.length > 0) { @@ -168,6 +169,7 @@ const pipelineTypesWrapper = computed<{ name: string; value: number }[]>(() => { { name: "Reflective", value: WebsocketPipelineType.Reflective }, { name: "Colored Shape", value: WebsocketPipelineType.ColoredShape }, { name: "AprilTag", value: WebsocketPipelineType.AprilTag }, + { name: "AprilTagCuda", value: WebsocketPipelineType.AprilTagCuda }, { name: "Aruco", value: WebsocketPipelineType.Aruco } ]; if (useSettingsStore().general.supportedBackends.length > 0) { @@ -224,6 +226,9 @@ useCameraSettingsStore().$subscribe((mutation, state) => { case PipelineType.AprilTag: pipelineType.value = WebsocketPipelineType.AprilTag; break; + case PipelineType.AprilTagCuda: + pipelineType.value = WebsocketPipelineType.AprilTagCuda; + break; case PipelineType.Aruco: pipelineType.value = WebsocketPipelineType.Aruco; break; diff --git a/photon-client/src/components/dashboard/ConfigOptions.vue b/photon-client/src/components/dashboard/ConfigOptions.vue index c1023f2172..bd750695a2 100644 --- a/photon-client/src/components/dashboard/ConfigOptions.vue +++ b/photon-client/src/components/dashboard/ConfigOptions.vue @@ -7,6 +7,7 @@ import InputTab from "@/components/dashboard/tabs/InputTab.vue"; import ThresholdTab from "@/components/dashboard/tabs/ThresholdTab.vue"; import ContoursTab from "@/components/dashboard/tabs/ContoursTab.vue"; import AprilTagTab from "@/components/dashboard/tabs/AprilTagTab.vue"; +import AprilTagCudaTab from "@/components/dashboard/tabs/AprilTagCudaTab.vue"; import ArucoTab from "@/components/dashboard/tabs/ArucoTab.vue"; import ObjectDetectionTab from "@/components/dashboard/tabs/ObjectDetectionTab.vue"; import OutputTab from "@/components/dashboard/tabs/OutputTab.vue"; @@ -37,6 +38,10 @@ const allTabs = Object.freeze({ tabName: "AprilTag", component: AprilTagTab }, + apriltagCudaTab: { + tabName: "AprilTagCuda", + component: AprilTagCudaTab + }, arucoTab: { tabName: "Aruco", component: ArucoTab @@ -79,6 +84,7 @@ const getTabGroups = (): ConfigOption[][] => { allTabs.thresholdTab, allTabs.contoursTab, allTabs.apriltagTab, + allTabs.apriltagCudaTab, allTabs.arucoTab, allTabs.objectDetectionTab, allTabs.outputTab @@ -92,6 +98,7 @@ const getTabGroups = (): ConfigOption[][] => { allTabs.thresholdTab, allTabs.contoursTab, allTabs.apriltagTab, + allTabs.apriltagCudaTab, allTabs.arucoTab, allTabs.objectDetectionTab, allTabs.outputTab @@ -102,7 +109,7 @@ const getTabGroups = (): ConfigOption[][] => { return [ [allTabs.inputTab], [allTabs.thresholdTab], - [allTabs.contoursTab, allTabs.apriltagTab, allTabs.arucoTab, allTabs.objectDetectionTab, allTabs.outputTab], + [allTabs.contoursTab, allTabs.apriltagTab, allTabs.apriltagCudaTab, allTabs.arucoTab, allTabs.objectDetectionTab, allTabs.outputTab], [allTabs.targetsTab, allTabs.pnpTab, allTabs.map3dTab] ]; } @@ -115,6 +122,7 @@ const tabGroups = computed(() => { const allow3d = useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled; const isAprilTag = useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.AprilTag; + const isAprilTagCuda = useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.AprilTagCuda; const isAruco = useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.Aruco; const isObjectDetection = useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.ObjectDetection; @@ -124,10 +132,11 @@ const tabGroups = computed(() => { tabGroup.filter( (tabConfig) => !(!allow3d && tabConfig.tabName === "3D") && //Filter out 3D tab any time 3D isn't calibrated - !((!allow3d || isAprilTag || isAruco || isObjectDetection) && tabConfig.tabName === "PnP") && //Filter out the PnP config tab if 3D isn't available, or we're doing AprilTags - !((isAprilTag || isAruco || isObjectDetection) && tabConfig.tabName === "Threshold") && //Filter out threshold tab if we're doing AprilTags - !((isAprilTag || isAruco || isObjectDetection) && tabConfig.tabName === "Contours") && //Filter out contours if we're doing AprilTags + !((!allow3d || isAprilTag || isAprilTagCuda || isAruco || isObjectDetection) && tabConfig.tabName === "PnP") && //Filter out the PnP config tab if 3D isn't available, or we're doing AprilTags + !((isAprilTag || isAprilTagCuda || isAruco || isObjectDetection) && tabConfig.tabName === "Threshold") && //Filter out threshold tab if we're doing AprilTags + !((isAprilTag || isAprilTagCuda || isAruco || isObjectDetection) && tabConfig.tabName === "Contours") && //Filter out contours if we're doing AprilTags !(!isAprilTag && tabConfig.tabName === "AprilTag") && //Filter out apriltag unless we actually are doing AprilTags + !(!isAprilTagCuda && tabConfig.tabName === "AprilTagCuda") && //Filter out apriltag unless we actually are doing AprilTags !(!isAruco && tabConfig.tabName === "Aruco") && !(!isObjectDetection && tabConfig.tabName === "Object Detection") //Filter out aruco unless we actually are doing Aruco ) diff --git a/photon-client/src/components/dashboard/tabs/OutputTab.vue b/photon-client/src/components/dashboard/tabs/OutputTab.vue index 3c3c84189b..896a95c4f7 100644 --- a/photon-client/src/components/dashboard/tabs/OutputTab.vue +++ b/photon-client/src/components/dashboard/tabs/OutputTab.vue @@ -10,6 +10,7 @@ import { useStateStore } from "@/stores/StateStore"; const isTagPipeline = computed( () => useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag || + useCameraSettingsStore().currentPipelineType === PipelineType.AprilTagCuda || useCameraSettingsStore().currentPipelineType === PipelineType.Aruco ); @@ -90,6 +91,7 @@ const interactiveCols = computed(() => { {