From d671236002a8580b9bfd8094655cb2494027512d Mon Sep 17 00:00:00 2001 From: amquake Date: Fri, 20 Oct 2023 17:36:33 -0700 Subject: [PATCH] update multitag result --- .gitignore | 3 +++ .../java/org/photonvision/PhotonCamera.java | 24 +++++++++++++++---- .../simulation/PhotonCameraSim.java | 22 +++++++++++++++-- 3 files changed, 43 insertions(+), 6 deletions(-) diff --git a/.gitignore b/.gitignore index 609770cdf5..766874b000 100644 --- a/.gitignore +++ b/.gitignore @@ -157,5 +157,8 @@ photonlib-java-examples/*/vendordeps/* photonlib-cpp-examples/*/vendordeps/* */networktables.json +*/networktables.json.bck +photonlib-cpp-examples/*/networktables.json.bck +photonlib-java-examples/*/networktables.json.bck *.sqlite photon-server/src/main/resources/web/index.html diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 9500629ca3..732d85a44b 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -27,6 +27,7 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.Matrix; @@ -121,6 +122,9 @@ public static void setVersionCheckEnabled(boolean enabled) { Packet packet = new Packet(1); + private static AprilTagFieldLayout lastSetTagLayout = + AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + // Existing is enough to make this multisubscriber do its thing private final MultiSubscriber m_topicNameSubscriber; @@ -332,9 +336,20 @@ public boolean isConnected() { } /** - * 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. + * Get the last set {@link AprilTagFieldLayout}. The tag layout is shared between all PhotonVision + * coprocessors on the same NT instance-- this method returns the most recent layout set. If no + * layout has been set by the user, this is equal to {@link AprilTagFields#kDefaultField}. + * + * @return The last set layout + */ + public AprilTagFieldLayout getAprilTagFieldLayout() { + return lastSetTagLayout; + } + + /** + * Set the {@link AprilTagFieldLayout} used by all PhotonVision coprocessors that are (or might + * later) connect to this robot. The topic is marked as persistent, 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. * @@ -342,10 +357,11 @@ public boolean isConnected() { * @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients * have updated their internal layouts. */ - public boolean setApriltagFieldLayout(AprilTagFieldLayout layout) { + public boolean setAprilTagFieldLayout(AprilTagFieldLayout layout) { try { var layout_json = new ObjectMapper().writeValueAsString(layout); atflPublisher.set(layout_json); + lastSetTagLayout = layout; return true; } catch (JsonProcessingException e) { MathSharedStore.reportError("Error setting ATFL in " + this.getName(), e.getStackTrace()); 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 1c9ef2ce1e..a0542c18b0 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -37,6 +37,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; +import java.util.stream.Collectors; import org.opencv.core.Core; import org.opencv.core.CvType; import org.opencv.core.Mat; @@ -52,6 +53,8 @@ import org.photonvision.estimation.OpenCVHelp; import org.photonvision.estimation.RotTrlTransform3d; import org.photonvision.estimation.TargetModel; +import org.photonvision.estimation.VisionEstimation; +import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.targeting.PNPResults; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -524,11 +527,26 @@ public PhotonPipelineResult process( videoSimProcessed.putFrame(videoSimFrameProcessed); } else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose); - // put this simulated data to NT + // calculate multitag results + var multitagResults = new MultiTargetPNPResults(); + var tagLayout = cam.getAprilTagFieldLayout(); + var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout); + if (visibleLayoutTags.size() > 1) { + List usedIDs = + visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList()); + var pnpResults = + VisionEstimation.estimateCamPosePNP( + prop.getIntrinsics(), prop.getDistCoeffs(), detectableTgts, tagLayout); + multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs); + } + + // sort target order if (sortMode != null) { detectableTgts.sort(sortMode.getComparator()); } - return new PhotonPipelineResult(latencyMillis, detectableTgts); + + // put this simulated data to NT + return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResults); } /**