From 29818c46f6c052a08a492e2d8eab9e8101a2033a Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 5 Nov 2023 23:39:16 -0500 Subject: [PATCH] fully working everything but not --- .../networktables/NTDataPublisher.java | 10 +- .../vision/pipe/impl/MultiTargetPNPPipe.java | 4 +- .../vision/target/TrackedTarget.java | 18 ++- .../java/org/photonvision/PhotonCamera.java | 35 ++--- .../org/photonvision/PhotonPoseEstimator.java | 2 + .../photonvision/PhotonTargetSortMode.java | 2 +- .../simulation/PhotonCameraSim.java | 2 + .../simulation/SimVisionSystem.java | 2 + .../native/cpp/photonlib/PhotonCamera.cpp | 6 +- .../include/photonlib/SimPhotonCamera.h | 3 +- .../java/org/photonvision/PacketTest.java | 3 + .../org/photonvision/PhotonCameraTest.java | 1 + .../photonvision/PhotonPoseEstimatorTest.java | 3 + photon-targeting/build.gradle | 1 - .../photonvision/estimation/OpenCVHelp.java | 24 ++- .../estimation/VisionEstimation.java | 31 ++-- .../targeting/MultiTargetPNPResults.java | 48 ++++-- .../photonvision/targeting/PNPResults.java | 63 +++++++- .../targeting/PhotonPipelineResult.java | 122 ++++++++++++--- .../targeting/PhotonTrackedTarget.java | 144 +++++++++++++----- .../photonvision/targeting/TargetCorner.java | 12 +- 21 files changed, 379 insertions(+), 157 deletions(-) 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 814decb804..7baf0a2f27 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 @@ -128,11 +128,11 @@ public void updateCameraNickname(String newCameraNickname) { @Override public void accept(CVPipelineResult result) { - var res = new PhotonPipelineResult( - result.getLatencyMillis(), - TrackedTarget.simpleFromTrackedTargets(result.targets), - result.multiTagResult - ); + var res = + new PhotonPipelineResult( + result.getLatencyMillis(), + TrackedTarget.simpleFromTrackedTargets(result.targets), + result.multiTagResult); ts.rawBytesEntry.set(res); 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 d243e37e6a..313d8a19e0 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 @@ -71,8 +71,8 @@ private MultiTargetPNPResults calculateCameraInField(List targetL VisionEstimation.estimateCamPosePNP( params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(), params.cameraCoefficients.distCoeffs.getAsWpilibMat(), - List.of(TrackedTarget.simpleFromTrackedTargets(targetList)), - params.atfl); + TrackedTarget.simpleFromTrackedTargets(targetList), + params.atfl, params.targetModel); return new MultiTargetPNPResults(estimatedPose, tagIDsUsed); 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 335e889464..2a28d05e56 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 @@ -113,10 +113,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 @@ -203,10 +204,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); var rvec = new Mat(3, 1, CvType.CV_64FC1); diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index e2265dcead..cd64965990 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -51,8 +51,7 @@ import java.util.Set; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.hardware.VisionLEDMode; -import org.photonvision.proto.PhotonTypes.PhotonPipelineResult; -import us.hebi.quickbuf.InvalidProtocolBufferException; +import org.photonvision.targeting.PhotonPipelineResult; /** Represents a camera that is connected to PhotonVision. */ public class PhotonCamera implements AutoCloseable { @@ -141,12 +140,9 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { path = cameraTable.getPath(); rawBytesEntry = cameraTable - .getRawTopic("result_proto") + .getRawTopic("rawBytes") .subscribe( - "proto:" + PhotonPipelineResult.getDescriptor().getFullName(), - new byte[] {}, - PubSubOption.periodic(0.01), - PubSubOption.sendAll(true)); + "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0); @@ -187,26 +183,23 @@ public PhotonCamera(String cameraName) { * @return The latest pipeline result. */ public PhotonPipelineResult getLatestResult() { - // verifyVersion(); // Protobufs _should_ deal with this for us + verifyVersion(); - var ret = PhotonPipelineResult.newInstance(); + // Clear the packet. + packet.clear(); - var bytes = rawBytesEntry.get(new byte[] {}); - if (bytes.length < 1) { - return PhotonPipelineResult.newInstance(); - } + // Create latest result. + var ret = new PhotonPipelineResult(); - try { - ret = PhotonPipelineResult.parseFrom(bytes); - } catch (InvalidProtocolBufferException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - return ret; - } + // Populate packet and create result. + packet.setData(rawBytesEntry.get(new byte[] {})); + + if (packet.getSize() < 1) return ret; + ret.createFromPacket(packet); // Set the timestamp of the result. // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds. - ret.setTimestampSec((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMs() / 1e3); + ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3); // Return result. return ret; diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 67e2d11f2d..32b4c7ba1f 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -43,6 +43,8 @@ import java.util.Set; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; /** * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a diff --git a/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java b/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java index 46c48ceb8a..bf22644645 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java @@ -25,7 +25,7 @@ package org.photonvision; import java.util.Comparator; -import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget; +import org.photonvision.targeting.PhotonTrackedTarget; public enum PhotonTargetSortMode { Smallest(Comparator.comparingDouble(PhotonTrackedTarget::getArea)), 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 9511300ab0..f725837e13 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -56,6 +56,8 @@ 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; /** * A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java index 3e6147625d..52756f4727 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java @@ -38,6 +38,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; import java.util.List; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; /** * @deprecated Use {@link VisionSystemSim} instead diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index 9c3fc733ce..d0901b10bd 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -40,9 +40,9 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, const std::string_view cameraName) : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), - rawBytesEntry(rootTable->GetRawTopic("result_proto") - .Subscribe("asdfasdfasdf", {}, - {.periodic = 0.01, .sendAll = true})), + rawBytesEntry( + rootTable->GetRawTopic("rawBytes") + .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})), inputSaveImgEntry( rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()), inputSaveImgSubscriber( diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h index 9d491b1fc3..44ab0917a2 100644 --- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h @@ -48,8 +48,7 @@ class SimPhotonCamera : public PhotonCamera { targetAreaEntry = rootTable->GetEntry("targetAreaEntry"); targetSkewEntry = rootTable->GetEntry("targetSkewEntry"); targetPoseEntry = rootTable->GetEntry("targetPoseEntry"); - rawBytesPublisher = - rootTable->GetRawTopic("result_proto").Publish("asdfasdfasdf"); + rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("rawBytes"); versionEntry = instance.GetTable("photonvision")->GetEntry("version"); // versionEntry.SetString(PhotonVersion.versionString); } diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java index 1224f0cff8..c29a629b5b 100644 --- a/photon-lib/src/test/java/org/photonvision/PacketTest.java +++ b/photon-lib/src/test/java/org/photonvision/PacketTest.java @@ -32,6 +32,9 @@ 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; class PacketTest { @Test diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index 77e18a6d8a..8a3fb5fad8 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -27,6 +27,7 @@ import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.Test; import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.targeting.PhotonPipelineResult; class PhotonCameraTest { @Test diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 6f1f523686..1b386cc747 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -47,6 +47,9 @@ import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.Test; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; class PhotonPoseEstimatorTest { static AprilTagFieldLayout aprilTags; diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 05c368b2c6..5c85d4bcb5 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -4,7 +4,6 @@ plugins { id 'edu.wpi.first.WpilibTools' version '1.3.0' } - apply from: "${rootDir}/shared/common.gradle" dependencies { diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index c253a198c2..84e5d8d7cc 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -30,8 +30,6 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.List; -import java.util.Optional; - import org.ejml.simple.SimpleMatrix; import org.opencv.calib3d.Calib3d; import org.opencv.core.Core; @@ -172,11 +170,8 @@ public static Point[] cornersToPoints(TargetCorner... corners) { public static List pointsToCorners(Point... points) { var corners = new ArrayList(points.length); - for (Point point : points) { - corners.add(new TargetCorner( - point.x, - point.y - )); + for (int i = 0; i < points.length; i++) { + corners.add(new TargetCorner(points[i].x, points[i].y)); } return corners; } @@ -406,7 +401,7 @@ public static Point[] getConvexHull(Point[] points) { * @return The resulting transformation that maps the camera pose to the target pose and the * ambiguity if an alternate solution is available. */ - public static Optional solvePNP_SQUARE( + public static PNPResults solvePNP_SQUARE( Matrix cameraMatrix, Matrix distCoeffs, List modelTrls, @@ -471,16 +466,15 @@ public static Optional solvePNP_SQUARE( // check if solvePnP failed with NaN results and retrying failed if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result"); - if (alt != null) - return Optional.of(new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1])); - else return Optional.of(new PNPResults(best, errors[0])); + return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]); + else return new PNPResults(best, errors[0]); } // solvePnP failed catch (Exception e) { System.err.println("SolvePNP_SQUARE failed!"); e.printStackTrace(); - return Optional.empty(); + return new PNPResults(); } finally { // release our Mats from native memory objectMat.release(); @@ -515,7 +509,7 @@ public static Optional solvePNP_SQUARE( * model points are supplied relative to the origin, this transformation brings the camera to * the origin. */ - public static Optional solvePNP_SQPNP( + public static PNPResults solvePNP_SQPNP( Matrix cameraMatrix, Matrix distCoeffs, List objectTrls, @@ -564,11 +558,11 @@ public static Optional solvePNP_SQPNP( // check if solvePnP failed with NaN results if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result"); - return Optional.of(new PNPResults(best, error[0])); + return new PNPResults(best, error[0]); } catch (Exception e) { System.err.println("SolvePNP_SQPNP failed!"); e.printStackTrace(); - return Optional.empty(); + return new PNPResults(); } } } diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index 2b13537be6..9ad6e0fc4e 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -27,7 +27,6 @@ import java.util.ArrayList; import java.util.List; import java.util.Objects; -import java.util.Optional; import java.util.stream.Collectors; import org.opencv.core.Point; import org.photonvision.targeting.PNPResults; @@ -67,7 +66,7 @@ public static List getVisibleLayoutTags( * @return The transformation that maps the field origin to the camera pose. Ensure the {@link * PNPResults} are present before utilizing them. */ - public static Optional estimateCamPosePNP( + public static PNPResults estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, List visTags, @@ -75,9 +74,9 @@ public static Optional estimateCamPosePNP( TargetModel tagModel) { if (tagLayout == null || visTags == null - || tagLayout.getTags().isEmpty() - || visTags.isEmpty()) { - return Optional.empty(); + || tagLayout.getTags().size() == 0 + || visTags.size() == 0) { + return new PNPResults(); } var corners = new ArrayList(); @@ -93,43 +92,41 @@ public static Optional estimateCamPosePNP( corners.addAll(tgt.getDetectedCorners()); }); } - if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) { - return Optional.empty(); + if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { + return new PNPResults(); } Point[] points = OpenCVHelp.cornersToPoints(corners); // single-tag pnp if (knownTags.size() == 1) { - var camToTagOpt = + var camToTag = OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points); - if (camToTagOpt.isEmpty()) return Optional.empty(); - var camToTag = camToTagOpt.get(); + if (!camToTag.isPresent) return new PNPResults(); var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse()); var altPose = new Pose3d(); if (camToTag.ambiguity != 0) altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse()); var o = new Pose3d(); - return Optional.of(new PNPResults( + return new PNPResults( new Transform3d(o, bestPose), new Transform3d(o, altPose), camToTag.ambiguity, camToTag.bestReprojErr, - camToTag.altReprojErr)); + camToTag.altReprojErr); } // multi-tag pnp else { var objectTrls = new ArrayList(); for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose)); - var camToOriginOpt = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points); - if (camToOriginOpt.isEmpty()) return Optional.empty(); - var camToOrigin = camToOriginOpt.get(); - return Optional.of(new PNPResults( + var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points); + if (!camToOrigin.isPresent) return new PNPResults(); + return new PNPResults( camToOrigin.best.inverse(), camToOrigin.alt.inverse(), camToOrigin.ambiguity, camToOrigin.bestReprojErr, - camToOrigin.altReprojErr)); + camToOrigin.altReprojErr); } } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java index 71c47c2765..f53353e820 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java @@ -18,21 +18,51 @@ package org.photonvision.targeting; import edu.wpi.first.util.protobuf.Protobuf; -import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResults; -import us.hebi.quickbuf.Descriptors.Descriptor; +import java.util.ArrayList; import java.util.Arrays; import java.util.List; import java.util.stream.Collectors; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResults; +import us.hebi.quickbuf.Descriptors.Descriptor; public class MultiTargetPNPResults { - public final PNPResults estimatedPose; - public final List fiducialIDsUsed; + // 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; @@ -66,7 +96,8 @@ public String toString() { + "]"; } - public static final class AProto implements Protobuf { + public static final class AProto + implements Protobuf { @Override public Class getTypeClass() { return MultiTargetPNPResults.class; @@ -87,10 +118,7 @@ public MultiTargetPNPResults unpack(ProtobufMultiTargetPNPResults msg) { return new MultiTargetPNPResults( PNPResults.proto.unpack(msg.getEstimatedPose()), // TODO better way of doing this - Arrays.stream(msg.getFiducialIdsUsed().array()) - .boxed() - .collect(Collectors.toList()) - ); + Arrays.stream(msg.getFiducialIdsUsed().array()).boxed().collect(Collectors.toList())); } @Override @@ -99,7 +127,7 @@ public void pack(ProtobufMultiTargetPNPResults msg, MultiTargetPNPResults value) // TODO better way of doing this int[] ids = new int[value.fiducialIDsUsed.size()]; - for(int i = 0; i < ids.length; i++) { + for (int i = 0; i < ids.length; i++) { ids[i] = value.fiducialIDsUsed.get(i); } msg.addAllFiducialIdsUsed(ids); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java index f2f4f853d2..819d48c577 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java @@ -19,7 +19,9 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.proto.PhotonTypes.ProtobufPNPResults; +import org.photonvision.utils.PacketUtils; import us.hebi.quickbuf.Descriptors.Descriptor; /** @@ -32,6 +34,12 @@ * 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. @@ -53,12 +61,27 @@ public class PNPResults { /** 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; @@ -66,14 +89,37 @@ public PNPResults( this.altReprojErr = altReprojErr; } - public PNPResults(Transform3d best, double bestReprojErr) { - this(best, best, 0, bestReprojErr, bestReprojErr); + 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); @@ -92,6 +138,7 @@ public boolean equals(Object obj) { 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; @@ -109,7 +156,9 @@ public boolean equals(Object obj) { @Override public String toString() { - return "PNPResults [best=" + return "PNPResults [isPresent=" + + isPresent + + ", best=" + best + ", bestReprojErr=" + bestReprojErr @@ -145,16 +194,16 @@ public PNPResults unpack(ProtobufPNPResults msg) { Transform3d.proto.unpack(msg.getAlt()), msg.getAmbiguity(), msg.getBestReprojErr(), - msg.getAltReprojErr() - ); + msg.getAltReprojErr()); } @Override public void pack(ProtobufPNPResults msg, PNPResults value) { Transform3d.proto.pack(msg.getMutableBest(), value.best); Transform3d.proto.pack(msg.getMutableAlt(), value.alt); - msg.setAmbiguity(value.ambiguity).setBestReprojErr(value.bestReprojErr).setAltReprojErr(value.altReprojErr); - + msg.setAmbiguity(value.ambiguity) + .setBestReprojErr(value.bestReprojErr) + .setAltReprojErr(value.altReprojErr); } } 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 f789738195..31bc7f2ea3 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -18,22 +18,44 @@ package org.photonvision.targeting; import edu.wpi.first.util.protobuf.Protobuf; -import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; -import org.photonvision.proto.PhotonTypes.ProtobufPhotonPipelineResult; -import us.hebi.quickbuf.Descriptors.Descriptor; import java.util.ArrayList; import java.util.List; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.proto.PhotonTypes.ProtobufPhotonPipelineResult; +import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedMessage; /** Represents a pipeline result from a PhotonCamera. */ public class PhotonPipelineResult { private static boolean HAS_WARNED = false; + // Targets to store. public final List targets = new ArrayList<>(); - private final double latencyMillis; - private final MultiTargetPNPResults multiTagResult; + // Latency in milliseconds. + private double latencyMillis; + + // Timestamp in milliseconds. private double timestampSeconds = -1; + // Multi-tag result + private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); + + /** Constructs an empty pipeline result. */ + public PhotonPipelineResult() {} + + /** + * Constructs a pipeline result. + * + * @param latencyMillis The latency in the pipeline. + * @param targets The list of targets identified by the pipeline. + */ + public PhotonPipelineResult(double latencyMillis, List targets) { + this.latencyMillis = latencyMillis; + this.targets.addAll(targets); + } + /** * Constructs a pipeline result. * @@ -48,6 +70,18 @@ public PhotonPipelineResult( 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 // latency + + MultiTargetPNPResults.PACK_SIZE_BYTES + + 1; // target count + } + /** * Returns the best target in this pipeline result. If there are no targets, this method will * return null. The best target is determined by the target sort mode in the PhotonVision UI. @@ -76,15 +110,6 @@ public double getLatencyMillis() { return latencyMillis; } - /** - * Sets the FPGA timestamp of this result in seconds. - * - * @param timestampSeconds The timestamp in seconds. - */ - public void setTimestampSeconds(double timestampSeconds) { - this.timestampSeconds = timestampSeconds; - } - /** * Returns the estimated time the frame was taken, This is more accurate than using * getLatencyMillis() @@ -95,13 +120,22 @@ public double getTimestampSeconds() { return timestampSeconds; } + /** + * Sets the FPGA timestamp of this result in seconds. + * + * @param timestampSeconds The timestamp in seconds. + */ + public void setTimestampSeconds(double timestampSeconds) { + this.timestampSeconds = timestampSeconds; + } + /** * Returns whether the pipeline has targets. * * @return Whether the pipeline has targets. */ public boolean hasTargets() { - return !targets.isEmpty(); + return targets.size() > 0; } /** @@ -121,6 +155,49 @@ public MultiTargetPNPResults getMultiTagResult() { return multiTagResult; } + /** + * Populates the fields of the pipeline result from the packet. + * + * @param packet The incoming packet. + * @return The incoming packet. + */ + 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(); + + // Decode the information of each target. + for (int i = 0; i < (int) targetCount; ++i) { + var target = new PhotonTrackedTarget(); + target.createFromPacket(packet); + targets.add(target); + } + + return packet; + } + + /** + * Populates the outgoing packet with information from this pipeline result. + * + * @param packet The outgoing packet. + * @return The outgoing 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. + for (var target : targets) target.populatePacket(packet); + + // Return the packet. + return packet; + } + @Override public int hashCode() { final int prime = 31; @@ -167,7 +244,8 @@ public String toString() { + "]"; } - public static final class AProto implements Protobuf { + public static final class AProto + implements Protobuf { @Override public Class getTypeClass() { return PhotonPipelineResult.class; @@ -188,15 +266,17 @@ public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { return new PhotonPipelineResult( msg.getLatencyMs(), PhotonTrackedTarget.proto.unpack(msg.getTargets()), - MultiTargetPNPResults.proto.unpack(msg.getMultiTargetResult()) - ); + MultiTargetPNPResults.proto.unpack(msg.getMultiTargetResult())); } @Override public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) { - ProtobufPhotonTrackedTarget[] targets = new ProtobufPhotonTrackedTarget[value.targets.size()]; - PhotonTrackedTarget.proto.pack(targets, value.targets); - msg.addAllTargets(targets); + RepeatedMessage targets = + msg.getMutableTargets().reserve(value.targets.size()); + for (int i = 0; i < value.targets.size(); i++) { + ProtobufPhotonTrackedTarget corner = targets.next(); + PhotonTrackedTarget.proto.pack(corner, value.targets.get(i)); + } MultiTargetPNPResults.proto.pack(msg.getMutableMultiTargetResult(), value.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 ed32cc1037..8fa4b75700 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -18,31 +18,37 @@ package org.photonvision.targeting; import edu.wpi.first.math.geometry.Transform3d; - +import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; -import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; import org.photonvision.proto.PhotonTypes.ProtobufTargetCorner; +import org.photonvision.utils.PacketUtils; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedMessage; public class PhotonTrackedTarget { private static final int MAX_CORNERS = 8; - - private final double yaw; - private final double pitch; - private final double area; - private final double skew; - private final int fiducialId; - private final Transform3d bestCameraToTarget; - private final Transform3d altCameraToTarget; - private final double poseAmbiguity; + public static final int PACK_SIZE_BYTES = + Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS); + + private double yaw; + private double pitch; + private double area; + private double skew; + private int fiducialId; + private Transform3d bestCameraToTarget = new Transform3d(); + private Transform3d altCameraToTarget = new Transform3d(); + private double poseAmbiguity; // Corners from the min-area rectangle bounding the target - private final List minAreaRectCorners; + private List minAreaRectCorners; + // Corners from whatever corner detection method was used - private final List detectedCorners; + private List detectedCorners; + + public PhotonTrackedTarget() {} /** Construct a tracked target, given exactly 4 corners */ public PhotonTrackedTarget( @@ -195,6 +201,81 @@ public boolean equals(Object obj) { return true; } + 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++) { + packet.encode(list.get(i).x); + packet.encode(list.get(i).y); + } + } + + private static List decodeList(Packet p) { + byte len = p.decodeByte(); + var ret = new ArrayList(); + for (int i = 0; i < len; i++) { + double cx = p.decodeDouble(); + double cy = p.decodeDouble(); + ret.add(new TargetCorner(cx, cy)); + } + return ret; + } + + /** + * Populates the fields of this class with information from the incoming packet. + * + * @param packet The incoming packet. + * @return The incoming packet. + */ + public Packet createFromPacket(Packet packet) { + this.yaw = packet.decodeDouble(); + this.pitch = packet.decodeDouble(); + this.area = packet.decodeDouble(); + this.skew = packet.decodeDouble(); + this.fiducialId = packet.decodeInt(); + + this.bestCameraToTarget = PacketUtils.decodeTransform(packet); + this.altCameraToTarget = PacketUtils.decodeTransform(packet); + + this.poseAmbiguity = packet.decodeDouble(); + + this.minAreaRectCorners = new ArrayList<>(4); + for (int i = 0; i < 4; i++) { + double cx = packet.decodeDouble(); + double cy = packet.decodeDouble(); + minAreaRectCorners.add(new TargetCorner(cx, cy)); + } + + detectedCorners = decodeList(packet); + + return packet; + } + + /** + * Populates the outgoing packet with information from the current target. + * + * @param packet The outgoing packet. + * @return The outgoing packet. + */ + public Packet populatePacket(Packet packet) { + packet.encode(yaw); + packet.encode(pitch); + packet.encode(area); + packet.encode(skew); + packet.encode(fiducialId); + PacketUtils.encodeTransform(packet, bestCameraToTarget); + PacketUtils.encodeTransform(packet, altCameraToTarget); + packet.encode(poseAmbiguity); + + for (int i = 0; i < 4; i++) { + packet.encode(minAreaRectCorners.get(i).x); + packet.encode(minAreaRectCorners.get(i).y); + } + + encodeList(packet, detectedCorners); + + return packet; + } + @Override public String toString() { return "PhotonTrackedTarget{" @@ -215,7 +296,8 @@ public String toString() { + '}'; } - public static final class AProto implements Protobuf { + public static final class AProto + implements Protobuf { @Override public Class getTypeClass() { return PhotonTrackedTarget.class; @@ -226,7 +308,6 @@ public Descriptor getDescriptor() { return ProtobufPhotonTrackedTarget.getDescriptor(); } - @Override public ProtobufPhotonTrackedTarget createMessage() { return ProtobufPhotonTrackedTarget.newInstance(); @@ -244,13 +325,12 @@ public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { Transform3d.proto.unpack(msg.getAltCameraToTarget()), msg.getPoseAmbiguity(), TargetCorner.proto.unpack(msg.getMinAreaRectCorners()), - TargetCorner.proto.unpack(msg.getDetectedCorners()) - ); + TargetCorner.proto.unpack(msg.getDetectedCorners())); } public List unpack(RepeatedMessage msg) { ArrayList targets = new ArrayList<>(msg.length()); - for(ProtobufPhotonTrackedTarget target : msg) { + for (ProtobufPhotonTrackedTarget target : msg) { targets.add(unpack(target)); } return targets; @@ -258,8 +338,7 @@ public List unpack(RepeatedMessage minAreaRectCorners = + msg.getMutableMinAreaRectCorners().reserve(value.minAreaRectCorners.size()); + for (int i = 0; i < value.minAreaRectCorners.size(); i++) { + ProtobufTargetCorner corner = minAreaRectCorners.next(); + TargetCorner.proto.pack(corner, value.minAreaRectCorners.get(i)); + } - public void pack(ProtobufPhotonTrackedTarget[] buffer, List targets) { - for(int i = 0; i < targets.size(); i++) { - var protoTarget = createMessage(); - pack(protoTarget, targets.get(i)); - buffer[i] = protoTarget; + RepeatedMessage detectedCorners = + msg.getMutableDetectedCorners().reserve(value.detectedCorners.size()); + for (int i = 0; i < value.detectedCorners.size(); i++) { + ProtobufTargetCorner corner = detectedCorners.next(); + TargetCorner.proto.pack(corner, value.detectedCorners.get(i)); } } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 1a655215f1..2cd1cfcc54 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -17,10 +17,10 @@ package org.photonvision.targeting; +import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; import java.util.Objects; -import edu.wpi.first.util.protobuf.Protobuf; import org.photonvision.proto.PhotonTypes.ProtobufTargetCorner; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedMessage; @@ -79,7 +79,7 @@ public TargetCorner unpack(ProtobufTargetCorner msg) { public List unpack(RepeatedMessage msg) { ArrayList corners = new ArrayList<>(msg.length()); - for(ProtobufTargetCorner corner : msg) { + for (ProtobufTargetCorner corner : msg) { corners.add(unpack(corner)); } return corners; @@ -89,14 +89,6 @@ public List unpack(RepeatedMessage msg) { public void pack(ProtobufTargetCorner msg, TargetCorner value) { msg.setX(value.x).setY(value.y); } - - public void pack(ProtobufTargetCorner[] buffer, List corners) { - for(int i = 0; i < corners.size(); i++) { - var protoCorner = createMessage(); - pack(protoCorner, corners.get(i)); - buffer[i] = protoCorner; - } - } } public static final AProto proto = new AProto();