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 d82fcdb86d..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 @@ -54,11 +54,13 @@ public void accept(CVPipelineResult result) { dataMap.put("targets", uiTargets); // Only send Multitag Results if they are present, similar to 3d pose - if (result.multiTagResult.isPresent) { + if (result.multiTagResult.estimatedPose.isPresent) { var multitagData = new HashMap(); multitagData.put( - "bestTransform", SerializationUtils.transformToHashMap(result.multiTagResult.best)); - multitagData.put("bestReprojectionError", result.multiTagResult.bestReprojErr); + "bestTransform", + SerializationUtils.transformToHashMap(result.multiTagResult.estimatedPose.best)); + multitagData.put("bestReprojectionError", result.multiTagResult.estimatedPose.bestReprojErr); + multitagData.put("fiducialIDsUsed", result.multiTagResult.fiducialIDsUsed); dataMap.put("multitagResult", multitagData); } 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 index eb052b03de..8d8693d607 100644 --- 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 @@ -26,22 +26,22 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.targeting.TargetCorner; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.pipe.CVPipe; -import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; /** Estimate the camera pose given multiple Apriltag observations */ public class MultiTargetPNPPipe - extends CVPipe, PNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> { + extends CVPipe< + List, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> { private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule); private boolean hasWarned = false; @Override - protected PNPResults process(List targetList) { + protected MultiTargetPNPResults process(List targetList) { if (params.cameraCoefficients == null || params.cameraCoefficients.getCameraIntrinsicsMat() == null || params.cameraCoefficients.getDistCoeffsMat() == null) { @@ -50,21 +50,22 @@ protected PNPResults process(List targetList) { "Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution..."); hasWarned = true; } - return new PNPResults(); + return new MultiTargetPNPResults(); } return calculateTargetPose(targetList); } - private PNPResults calculateTargetPose(List targetList) { + private MultiTargetPNPResults calculateTargetPose(List targetList) { if (params.cameraCoefficients == null || params.cameraCoefficients.getCameraIntrinsicsMat() == null || params.cameraCoefficients.getDistCoeffsMat() == null) { - return new PNPResults(); + return new MultiTargetPNPResults(); } var visCorners = new ArrayList(); var knownVisTags = new ArrayList(); + var tagIDsUsed = new ArrayList(); for (var target : targetList) { for (var corner : target.getTargetCorners()) { visCorners.add(new TargetCorner(corner.x, corner.y)); @@ -73,15 +74,17 @@ private PNPResults calculateTargetPose(List targetList) { // actual layout poses of visible tags -- not exposed, so have to recreate knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose)); + tagIDsUsed.add(target.getFiducialId()); } - var pnpResults = + var ret = new MultiTargetPNPResults(); + ret.estimatedPose = VisionEstimation.estimateCamPosePNP( params.cameraCoefficients.getCameraIntrinsicsMat(), params.cameraCoefficients.getDistCoeffsMat(), visCorners, knownVisTags); - - return pnpResults; + ret.fiducialIDsUsed = tagIDsUsed; + return ret; } Mat rotationMatrix = new Mat(); @@ -92,15 +95,11 @@ private PNPResults calculateTargetPose(List targetList) { public static class MultiTargetPNPPipeParams { private final CameraCalibrationCoefficients cameraCoefficients; - private final TargetModel targetModel; private final AprilTagFieldLayout atfl; public MultiTargetPNPPipeParams( - CameraCalibrationCoefficients cameraCoefficients, - TargetModel targetModel, - AprilTagFieldLayout atfl) { + CameraCalibrationCoefficients cameraCoefficients, AprilTagFieldLayout atfl) { this.cameraCoefficients = cameraCoefficients; - this.targetModel = targetModel; 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 80852142e8..92f14d577d 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 @@ -27,7 +27,7 @@ import java.util.List; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.util.math.MathUtils; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.pipe.CVPipe.CVPipeResult; @@ -109,8 +109,7 @@ protected void setPipeParamsImpl() { // TODO global state ew var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout(); multiTagPNPPipe.setParams( - new MultiTargetPNPPipeParams( - frameStaticProperties.cameraCalibration, settings.targetModel, atfl)); + new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl)); } } } @@ -127,7 +126,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting if (frame.type != FrameThresholdType.GREYSCALE) { // 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(), new PNPResults(), frame); + return new CVPipelineResult(0, 0, List.of(), new MultiTargetPNPResults(), frame); } CVPipeResult> tagDetectionPipeResult; @@ -167,7 +166,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting targetList.add(target); } - PNPResults multiTagResult = new PNPResults(); + MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); if (settings.solvePNPEnabled && settings.doMultiTarget) { var multiTagOutput = multiTagPNPPipe.run(targetList); sumPipeNanosElapsed += multiTagOutput.nanosElapsed; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java index 09b35c92e2..90bb6430ee 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java @@ -38,7 +38,7 @@ import java.util.ArrayList; import java.util.List; import org.opencv.core.Mat; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.vision.aruco.ArucoDetectionResult; import org.photonvision.vision.aruco.ArucoDetectorParams; import org.photonvision.vision.frame.Frame; @@ -97,7 +97,8 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) CVPipeResult> tagDetectionPipeResult; if (rawInputMat.empty()) { - return new CVPipelineResult(sumPipeNanosElapsed, 0, List.of(), new PNPResults(), frame); + return new CVPipelineResult( + sumPipeNanosElapsed, 0, List.of(), new MultiTargetPNPResults(), frame); } tagDetectionPipeResult = arucoDetectionPipe.run(rawInputMat); @@ -124,6 +125,7 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) var fpsResult = calculateFPSPipe.run(null); var fps = fpsResult.output; - return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, new PNPResults(), frame); + return new CVPipelineResult( + sumPipeNanosElapsed, fps, targetList, new MultiTargetPNPResults(), frame); } } 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 7e0cca3ebd..72154a9d5c 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,7 +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.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -111,7 +111,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se Mat inputColorMat = frame.colorImage.getMat(); if (this.calibrating || inputColorMat.empty()) { - return new CVPipelineResult(0, 0, null, new PNPResults(), frame); + return new CVPipelineResult(0, 0, null, new MultiTargetPNPResults(), frame); } long sumPipeNanosElapsed = 0L; @@ -147,7 +147,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se sumPipeNanosElapsed, fps, // Unused but here in case Collections.emptyList(), - new PNPResults(), + new MultiTargetPNPResults(), new Frame( new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties)); } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java index 657a2fd456..acf4674f97 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java @@ -22,7 +22,7 @@ import java.util.stream.Collectors; import org.apache.commons.lang3.tuple.Pair; import org.opencv.core.Point; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.opencv.*; @@ -227,6 +227,7 @@ protected CVPipelineResult process(Frame frame, ColoredShapePipelineSettings set var fpsResult = calculateFPSPipe.run(null); var fps = fpsResult.output; - return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, new PNPResults(), frame); + return new CVPipelineResult( + sumPipeNanosElapsed, fps, targetList, new MultiTargetPNPResults(), frame); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java index 1ca74444eb..666ecd8ce2 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java @@ -19,7 +19,6 @@ import java.util.List; import org.apache.commons.lang3.tuple.Pair; -import org.photonvision.targeting.PNPResults; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.opencv.DualOffsetValues; @@ -225,7 +224,6 @@ public CVPipelineResult process( sumPipeNanosElapsed, fps, // Unused but here just in case targetsToDraw, - new PNPResults(), inputAndOutputFrame); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java index 2f71c8e87b..cea2e31422 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java @@ -18,7 +18,6 @@ package org.photonvision.vision.pipeline; import java.util.List; -import org.photonvision.targeting.PNPResults; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.opencv.Contour; @@ -194,6 +193,6 @@ public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings PipelineProfiler.printReflectiveProfile(pipeProfileNanos); - return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, new PNPResults(), frame); + return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, frame); } } 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 d77b50a0aa..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,7 +20,7 @@ import java.util.Collections; import java.util.List; import org.photonvision.common.util.math.MathUtils; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.target.TrackedTarget; @@ -31,13 +31,18 @@ public class CVPipelineResult implements Releasable { public final double fps; public final List targets; public final Frame inputAndOutputFrame; - public PNPResults multiTagResult; + 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, - PNPResults multiTagResults, + MultiTargetPNPResults multiTagResults, Frame inputFrame) { this.processingNanos = processingNanos; this.fps = fps; @@ -48,7 +53,10 @@ public CVPipelineResult( } public CVPipelineResult( - double processingNanos, double fps, List targets, PNPResults multiTagResults) { + double processingNanos, + double fps, + List targets, + MultiTargetPNPResults multiTagResults) { this(processingNanos, fps, targets, multiTagResults, null); } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java index 98340d266a..609a3f4483 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java @@ -18,11 +18,10 @@ package org.photonvision.vision.pipeline.result; import java.util.List; -import org.photonvision.targeting.PNPResults; import org.photonvision.vision.frame.Frame; public class DriverModePipelineResult extends CVPipelineResult { public DriverModePipelineResult(double latencyNanos, double fps, Frame outputFrame) { - super(latencyNanos, fps, List.of(), new PNPResults(), outputFrame); + super(latencyNanos, fps, List.of(), outputFrame); } } 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 3f53b34a14..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,7 +35,7 @@ import org.photonvision.PhotonTargetSortMode; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.networktables.NTTopicSet; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -142,7 +142,7 @@ public void submitProcessedFrame( } PhotonPipelineResult newResult = - new PhotonPipelineResult(latencyMillis, targetList, new PNPResults()); + new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults()); var newPacket = new Packet(newResult.getPacketSize()); newResult.populatePacket(newPacket); ts.rawBytesEntry.set(newPacket.getData()); 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..92baf1a363 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java @@ -0,0 +1,41 @@ +package org.photonvision.targeting; + +import java.util.ArrayList; +import java.util.List; +import org.photonvision.common.dataflow.structures.Packet; + +public class MultiTargetPNPResults { + // pnpresult + 32 possible targets (arbitrary upper limit that should never be hit, ideally) + private static final int MAX_IDS = 32; + public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + Byte.BYTES * MAX_IDS; + + public PNPResults estimatedPose = new PNPResults(); + public List fiducialIDsUsed = List.of(); + + public MultiTargetPNPResults() {} + + public MultiTargetPNPResults(PNPResults results, ArrayList 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++) { + ids.add((int) packet.decodeByte()); + } + 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(fiducialIDsUsed.get(i).byteValue()); + } else { + packet.encode(Byte.MIN_VALUE); + } + } + } +} 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 36256e2602..056fe3877a 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -35,7 +35,7 @@ public class PhotonPipelineResult { private double timestampSeconds = -1; // Multi-tag result - private PNPResults multiTagResult = new PNPResults(); + private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); /** Constructs an empty pipeline result. */ public PhotonPipelineResult() {} @@ -59,7 +59,7 @@ public PhotonPipelineResult(double latencyMillis, List targ * @param result Result from multi-target PNP. */ public PhotonPipelineResult( - double latencyMillis, List targets, PNPResults results) { + double latencyMillis, List targets, MultiTargetPNPResults results) { this.latencyMillis = latencyMillis; this.targets.addAll(targets); this.multiTagResult = results; @@ -74,7 +74,7 @@ public int getPacketSize() { return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES + 8 + 2 - + PNPResults.PACK_SIZE_BYTES; + + MultiTargetPNPResults.PACK_SIZE_BYTES; } /** @@ -143,10 +143,10 @@ public List getTargets() { } /** - * Return the latest mulit-target result. Be sure to check PNPResults::isPresent before using the - * pose estimate! + * Return the latest mulit-target result. Be sure to check MultiTargetPNPResults::isPresent before + * using the pose estimate! */ - public PNPResults getMultiTagPnpResults() { + public MultiTargetPNPResults getMultiTagMultiTargetPNPResults() { return multiTagResult; } @@ -159,7 +159,7 @@ public PNPResults getMultiTagPnpResults() { public Packet createFromPacket(Packet packet) { // Decode latency, existence of targets, and number of targets. latencyMillis = packet.decodeDouble(); - this.multiTagResult = PNPResults.createFromPacket(packet); + this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet); byte targetCount = packet.decodeByte(); targets.clear();