From acaefbaed62ef5427228184790a3098094c7a4f9 Mon Sep 17 00:00:00 2001 From: Matt Date: Sat, 21 Oct 2023 10:07:38 -0400 Subject: [PATCH] Maybe sorta half working --- photon-core/build.gradle | 1 + .../networktables/NTDataPublisher.java | 27 +- .../vision/pipe/impl/MultiTargetPNPPipe.java | 2 +- .../vision/target/TrackedTarget.java | 71 ++- .../org/photonvision/EstimatedRobotPose.java | 2 +- .../java/org/photonvision/PhotonCamera.java | 33 +- .../org/photonvision/PhotonPoseEstimator.java | 2 - .../photonvision/PhotonTargetSortMode.java | 3 +- .../simulation/PhotonCameraSim.java | 2 - .../simulation/SimVisionSystem.java | 2 - .../native/cpp/photonlib/PhotonCamera.cpp | 4 +- .../include/photonlib/SimPhotonCamera.h | 2 +- .../java/org/photonvision/PacketTest.java | 3 - .../org/photonvision/PhotonCameraTest.java | 1 - .../photonvision/PhotonPoseEstimatorTest.java | 3 - photon-targeting/build.gradle | 38 ++ .../common/networktables/NTTopicSet.java | 10 +- .../photonvision/estimation/OpenCVHelp.java | 16 +- .../estimation/VisionEstimation.java | 8 +- .../targeting/MultiTargetPNPResults.java | 22 + .../targeting/PhotonPipelineResult.java | 484 ++++++++------- .../targeting/PhotonPipelineResultProto.java | 36 ++ .../targeting/PhotonTrackedTarget.java | 586 +++++++++--------- .../photonvision/targeting/TargetCorner.java | 90 +-- .../src/main/proto/photon_types.proto | 44 ++ 25 files changed, 838 insertions(+), 654 deletions(-) create mode 100644 photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java create mode 100644 photon-targeting/src/main/proto/photon_types.proto diff --git a/photon-core/build.gradle b/photon-core/build.gradle index 5ebef599a4..314c4adca8 100644 --- a/photon-core/build.gradle +++ b/photon-core/build.gradle @@ -29,6 +29,7 @@ dependencies { implementation "org.xerial:sqlite-jdbc:3.41.0.0" } + task writeCurrentVersionJava { def versionFileIn = file("${rootDir}/shared/PhotonVersion.java.in") writePhotonVersionFile(versionFileIn, Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java"), 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 d985c710f5..2951a96288 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 @@ -27,7 +27,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.networktables.NTTopicSet; -import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TrackedTarget; @@ -129,15 +129,22 @@ public void updateCameraNickname(String newCameraNickname) { @Override public void accept(CVPipelineResult result) { - var simplified = - new PhotonPipelineResult( - result.getLatencyMillis(), - TrackedTarget.simpleFromTrackedTargets(result.targets), - result.multiTagResult); - Packet packet = new Packet(simplified.getPacketSize()); - simplified.populatePacket(packet); - - ts.rawBytesEntry.set(packet.getData()); + // var simplified = + // new PhotonPipelineResult( + // result.getLatencyMillis(), + // TrackedTarget.simpleFromTrackedTargets(result.targets), + // result.multiTagResult); + // Packet packet = new Packet(simplified.getPacketSize()); + // simplified.populatePacket(packet); + + PhotonPipelineResult resultProto = PhotonPipelineResult.newInstance(); + resultProto.setLatencyMs(result.getLatencyMillis()); + resultProto.addAllTargets(TrackedTarget.simpleFromTrackedTargets(result.targets)); + if (result.multiTagResult.estimatedPose.isPresent) { + resultProto.setMultiTargetResult(result.multiTagResult.toProto()); + } + + ts.rawBytesEntry.set(resultProto); ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get()); ts.driverModePublisher.set(driverModeSupplier.getAsBoolean()); 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 5ce9f124cc..a37c2b24c3 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 @@ -69,7 +69,7 @@ private MultiTargetPNPResults calculateCameraInField(List targetL VisionEstimation.estimateCamPosePNP( params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(), params.cameraCoefficients.distCoeffs.getAsWpilibMat(), - TrackedTarget.simpleFromTrackedTargets(targetList), + List.of(TrackedTarget.simpleFromTrackedTargets(targetList)), params.atfl); 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 9be1e9162c..1a136d2ffe 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -22,6 +22,8 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d; + import java.util.ArrayList; import java.util.HashMap; import java.util.List; @@ -33,8 +35,8 @@ import org.opencv.core.RotatedRect; import org.photonvision.common.util.SerializationUtils; import org.photonvision.common.util.math.MathUtils; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.TargetCorner; import org.photonvision.vision.aruco.ArucoDetectionResult; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.opencv.CVShape; @@ -396,37 +398,64 @@ public boolean isFiducial() { return this.m_fiducialId >= 0; } - public static List simpleFromTrackedTargets(List targets) { - var ret = new ArrayList(); - for (var t : targets) { - var minAreaRectCorners = new ArrayList(); - var detectedCorners = new ArrayList(); + public static PhotonTrackedTarget[] simpleFromTrackedTargets(List targets) { + var ret = new PhotonTrackedTarget[targets.size()]; + for (int tgtIdx = 0; tgtIdx < targets.size(); tgtIdx++) { + var t = targets.get(tgtIdx); + var minAreaRectCorners = new TargetCorner[4]; + var detectedCorners = new TargetCorner[t.getTargetCorners().size()]; { var points = new Point[4]; t.getMinAreaRect().points(points); for (int i = 0; i < 4; i++) { - minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y)); + var c = TargetCorner.newInstance(); + c.setX(points[i].x); + c.setY(points[i].y); + minAreaRectCorners[i] = c; } } { var points = t.getTargetCorners(); for (int i = 0; i < points.size(); i++) { - detectedCorners.add(new TargetCorner(points.get(i).x, points.get(i).y)); + var c = TargetCorner.newInstance(); + c.setX(points.get(i).x); + c.setY(points.get(i).y); + detectedCorners[i] = c; } } - ret.add( - new PhotonTrackedTarget( - t.getYaw(), - t.getPitch(), - t.getArea(), - t.getSkew(), - t.getFiducialId(), - t.getBestCameraToTarget3d(), - t.getAltCameraToTarget3d(), - t.getPoseAmbiguity(), - minAreaRectCorners, - detectedCorners)); + var tt = PhotonTrackedTarget.newInstance(); + + var best = Transform3d.proto.createMessage(); + var alt = Transform3d.proto.createMessage(); + Transform3d.proto.pack(best, t.getBestCameraToTarget3d()); + Transform3d.proto.pack(alt, t.getAltCameraToTarget3d()); + + tt.setYaw(t.getYaw()); + tt.setPitch(t.getPitch()); + tt.setArea(t.getArea()); + tt.setSkew(t.getSkew()); + tt.setFiducialId(t.getFiducialId()); + tt.setBestCameraToTarget(best); + tt.setAltCameraToTarget(alt); + tt.setPoseAmbiguity(t.getPoseAmbiguity()); + tt.addAllMinAreaRectCorners(minAreaRectCorners); + tt.addAllDetectedCorners(detectedCorners); + + ret[tgtIdx] = tt; + + // ret.add( + // new PhotonTrackedTarget( + // t.getYaw(), + // t.getPitch(), + // t.getArea(), + // t.getSkew(), + // t.getFiducialId(), + // t.getBestCameraToTarget3d(), + // t.getAltCameraToTarget3d(), + // t.getPoseAmbiguity(), + // minAreaRectCorners, + // detectedCorners)); } return ret; } diff --git a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java index a355ab00f3..2b5d65253c 100644 --- a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java +++ b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java @@ -27,7 +27,7 @@ import edu.wpi.first.math.geometry.Pose3d; import java.util.List; import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget; /** An estimated pose based on pipeline result */ public class EstimatedRobotPose { diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 9500629ca3..dfac7e2e97 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -49,11 +49,13 @@ import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import us.hebi.quickbuf.InvalidProtocolBufferException; + import java.util.Optional; import java.util.Set; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.hardware.VisionLEDMode; -import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult; /** Represents a camera that is connected to PhotonVision. */ public class PhotonCamera implements AutoCloseable { @@ -139,9 +141,9 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { path = cameraTable.getPath(); rawBytesEntry = cameraTable - .getRawTopic("rawBytes") + .getRawTopic("result_proto") .subscribe( - "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + "proto:" + PhotonPipelineResult.getDescriptor().getFullName(), 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); @@ -182,23 +184,26 @@ public PhotonCamera(String cameraName) { * @return The latest pipeline result. */ public PhotonPipelineResult getLatestResult() { - verifyVersion(); - - // Clear the packet. - packet.clear(); + // verifyVersion(); // Protobufs _should_ deal with this for us - // Create latest result. - var ret = new PhotonPipelineResult(); + var ret = PhotonPipelineResult.newInstance(); - // Populate packet and create result. - packet.setData(rawBytesEntry.get(new byte[] {})); + var bytes = rawBytesEntry.get(new byte[] {}); + if (bytes.length < 1) { + return PhotonPipelineResult.newInstance(); + } - if (packet.getSize() < 1) return ret; - ret.createFromPacket(packet); + try { + ret = PhotonPipelineResult.parseFrom(bytes); + } catch (InvalidProtocolBufferException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + return ret; + } // Set the timestamp of the result. // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds. - ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3); + ret.setTimestampSec((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMs() / 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 ed29a9c265..d75799b39f 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -42,8 +42,6 @@ import java.util.Optional; import java.util.Set; 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 bf22644645..958e009abd 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java @@ -25,7 +25,8 @@ package org.photonvision; import java.util.Comparator; -import org.photonvision.targeting.PhotonTrackedTarget; + +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.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 1c9ef2ce1e..ebd6a53c19 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -53,8 +53,6 @@ import org.photonvision.estimation.RotTrlTransform3d; import org.photonvision.estimation.TargetModel; import org.photonvision.targeting.PNPResults; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; /** * 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 52756f4727..3e6147625d 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java @@ -38,8 +38,6 @@ 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 d0901b10bd..c4b55424ef 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -41,8 +41,8 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), rawBytesEntry( - rootTable->GetRawTopic("rawBytes") - .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})), + rootTable->GetRawTopic("result_proto") + .Subscribe("asdfasdfasdf", {}, {.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 44ab0917a2..dac3ba8e04 100644 --- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h @@ -48,7 +48,7 @@ class SimPhotonCamera : public PhotonCamera { targetAreaEntry = rootTable->GetEntry("targetAreaEntry"); targetSkewEntry = rootTable->GetEntry("targetSkewEntry"); targetPoseEntry = rootTable->GetEntry("targetPoseEntry"); - rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("rawBytes"); + rawBytesPublisher = rootTable->GetRawTopic("result_proto").Publish("asdfasdfasdf"); 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 c29a629b5b..1224f0cff8 100644 --- a/photon-lib/src/test/java/org/photonvision/PacketTest.java +++ b/photon-lib/src/test/java/org/photonvision/PacketTest.java @@ -32,9 +32,6 @@ 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 8a3fb5fad8..77e18a6d8a 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -27,7 +27,6 @@ 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 1b386cc747..6f1f523686 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -47,9 +47,6 @@ 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 e4cc2d7d96..bb8c8b08a4 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -1,3 +1,6 @@ +plugins { + id 'com.google.protobuf' version '0.9.3' +} apply plugin: "java" apply from: "${rootDir}/shared/common.gradle" @@ -13,6 +16,16 @@ dependencies { testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2") } +sourceSets { + main { + java { + // TODO this is a hack! I think! Why are we generating wpilib quickbufs anyways? + srcDirs 'build/generated/source/proto/main/quickbuf/org' + exclude 'build/generated/source/proto/main/quickbuf/edu/wpi/**' + } + } +} + tasks.withType(JavaCompile) { options.encoding = 'UTF-8' } @@ -29,4 +42,29 @@ test { useJUnitPlatform() } + +protobuf { + protoc { + artifact = 'com.google.protobuf:protoc:3.21.12' + } + plugins { + quickbuf { + artifact = 'us.hebi.quickbuf:protoc-gen-quickbuf:1.3.2' + } + } + generateProtoTasks { + all().configureEach { task -> + task.builtins { + cpp {} + remove java + } + task.plugins { + quickbuf { + option "gen_descriptors=true" + } + } + } + } +} + apply from: "publish.gradle" diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 5a0d31eaed..c4e817c9c1 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -17,6 +17,9 @@ package org.photonvision.common.networktables; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult; +import org.photonvision.targeting.PhotonPipelineResultProto; + import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.BooleanTopic; @@ -26,6 +29,7 @@ import edu.wpi.first.networktables.IntegerSubscriber; import edu.wpi.first.networktables.IntegerTopic; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.ProtobufPublisher; import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.RawPublisher; @@ -39,7 +43,7 @@ */ public class NTTopicSet { public NetworkTable subTable; - public RawPublisher rawBytesEntry; + public ProtobufPublisher rawBytesEntry; public IntegerPublisher pipelineIndexPublisher; public IntegerSubscriber pipelineIndexRequestSub; @@ -71,8 +75,8 @@ public class NTTopicSet { public void updateEntries() { rawBytesEntry = subTable - .getRawTopic("rawBytes") - .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + .getProtobufTopic("result_proto", PhotonPipelineResultProto.proto) + .publish(PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish(); pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0); 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 3f89918d62..80f18943fc 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -45,8 +45,8 @@ import org.opencv.core.Rect; import org.opencv.core.RotatedRect; import org.opencv.imgproc.Imgproc; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.TargetCorner; import org.photonvision.targeting.PNPResults; -import org.photonvision.targeting.TargetCorner; public final class OpenCVHelp { private static RotTrlTransform3d NWU_TO_EDN; @@ -160,7 +160,7 @@ public static Point[] cornersToPoints(List corners) { var points = new Point[corners.size()]; for (int i = 0; i < corners.size(); i++) { var corn = corners.get(i); - points[i] = new Point(corn.x, corn.y); + points[i] = new Point(corn.getX(), corn.getY()); } return points; } @@ -168,7 +168,7 @@ public static Point[] cornersToPoints(List corners) { public static Point[] cornersToPoints(TargetCorner... corners) { var points = new Point[corners.length]; for (int i = 0; i < corners.length; i++) { - points[i] = new Point(corners[i].x, corners[i].y); + points[i] = new Point(corners[i].getX(), corners[i].getY()); } return points; } @@ -176,7 +176,10 @@ public static Point[] cornersToPoints(TargetCorner... corners) { public static List pointsToCorners(Point... points) { var corners = new ArrayList(points.length); for (int i = 0; i < points.length; i++) { - corners.add(new TargetCorner(points[i].x, points[i].y)); + var c = TargetCorner.newInstance(); + c.setX(points[i].x); + c.setY(points[i].y); + corners.add(c); } return corners; } @@ -186,7 +189,10 @@ public static List pointsToCorners(MatOfPoint2f matInput) { float[] data = new float[(int) matInput.total() * matInput.channels()]; matInput.get(0, 0, data); for (int i = 0; i < (int) matInput.total(); i++) { - corners.add(new TargetCorner(data[0 + 2 * i], data[1 + 2 * i])); + var c = TargetCorner.newInstance(); + c.setX(data[0 + 2 * i]); + c.setY(data[1 + 2 * i]); + corners.add(c); } return corners; } 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 387176d57b..760c7a23fa 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -29,9 +29,9 @@ import java.util.Objects; import java.util.stream.Collectors; import org.opencv.core.Point; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.TargetCorner; import org.photonvision.targeting.PNPResults; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; public class VisionEstimation { /** Get the visible {@link AprilTag}s which are in the tag layout using the visible tag IDs. */ @@ -88,7 +88,9 @@ public static PNPResults estimateCamPosePNP( .ifPresent( pose -> { knownTags.add(new AprilTag(id, pose)); - corners.addAll(tgt.getDetectedCorners()); + for (var c : tgt.getDetectedCorners()) { + corners.add(c); + } }); } if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { 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 453c0a57e5..0d2fa0b214 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java @@ -21,6 +21,8 @@ import java.util.List; import org.photonvision.common.dataflow.structures.Packet; +import edu.wpi.first.math.geometry.Transform3d; + public class MultiTargetPNPResults { // Seeing 32 apriltags at once seems like a sane limit private static final int MAX_IDS = 32; @@ -90,4 +92,24 @@ public String toString() { + fiducialIDsUsed + "]"; } + + public org.photonvision.proto.PhotonTypes.PhotonPipelineResult.MultiTargetPNPResults toProto() { + var ret = org.photonvision.proto.PhotonTypes.PhotonPipelineResult.MultiTargetPNPResults.newInstance(); + + var best = Transform3d.proto.createMessage(); + var alt = Transform3d.proto.createMessage(); + Transform3d.proto.pack(best, this.estimatedPose.best); + Transform3d.proto.pack(alt, this.estimatedPose.alt); + ret.setBest(best); + ret.setAlt(alt); + ret.setBestReprojErr(this.estimatedPose.bestReprojErr); + ret.setAltReprojErr(this.estimatedPose.altReprojErr); + ret.setAmbiguity(this.estimatedPose.ambiguity); + int[] ids = new int[this.fiducialIDsUsed.size()]; + + for (int i = 0; i < ids.length; i++) ids[i] = this.fiducialIDsUsed.get(i); + ret.addAllFiducialIdsUsed(ids); + + return ret; + } } 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 4472078f8a..a6f51f6241 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -1,241 +1,243 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -package org.photonvision.targeting; - -import java.util.ArrayList; -import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; - -/** 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<>(); - - // 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. - * - * @param latencyMillis The latency in the pipeline. - * @param targets The list of targets identified by the pipeline. - * @param result Result from multi-target PNP. - */ - public PhotonPipelineResult( - double latencyMillis, List targets, MultiTargetPNPResults result) { - this.latencyMillis = latencyMillis; - this.targets.addAll(targets); - this.multiTagResult = result; - } - - /** - * Returns the size of the packet needed to store this pipeline result. - * - * @return The size of the packet needed to store this pipeline result. - */ - public int getPacketSize() { - return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES - + 8 // 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. - * - * @return The best target of the pipeline result. - */ - public PhotonTrackedTarget getBestTarget() { - if (!hasTargets() && !HAS_WARNED) { - String errStr = - "This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() " - + "before calling this method. For more information, please review the PhotonLib " - + "documentation at http://docs.photonvision.org"; - System.err.println(errStr); - new Exception().printStackTrace(); - HAS_WARNED = true; - } - return hasTargets() ? targets.get(0) : null; - } - - /** - * Returns the latency in the pipeline. - * - * @return The latency in the pipeline. - */ - public double getLatencyMillis() { - return latencyMillis; - } - - /** - * Returns the estimated time the frame was taken, This is more accurate than using - * getLatencyMillis() - * - * @return The timestamp in seconds, or -1 if this result has no timestamp set. - */ - 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.size() > 0; - } - - /** - * Returns a copy of the vector of targets. - * - * @return A copy of the vector of targets. - */ - public List getTargets() { - return new ArrayList<>(targets); - } - - /** - * Return the latest mulit-target result. Be sure to check - * getMultiTagResult().estimatedPose.isPresent before using the pose estimate! - */ - public MultiTargetPNPResults getMultiTagResult() { - return multiTagResult; - } - - /** - * Populates the fields of the pipeline result from the packet. - * - * @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; - int result = 1; - result = prime * result + ((targets == null) ? 0 : targets.hashCode()); - long temp; - temp = Double.doubleToLongBits(latencyMillis); - result = prime * result + (int) (temp ^ (temp >>> 32)); - temp = Double.doubleToLongBits(timestampSeconds); - result = prime * result + (int) (temp ^ (temp >>> 32)); - result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode()); - return result; - } - - @Override - public boolean equals(Object obj) { - if (this == obj) return true; - if (obj == null) return false; - if (getClass() != obj.getClass()) return false; - PhotonPipelineResult other = (PhotonPipelineResult) obj; - if (targets == null) { - if (other.targets != null) return false; - } else if (!targets.equals(other.targets)) return false; - if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) - return false; - if (Double.doubleToLongBits(timestampSeconds) - != Double.doubleToLongBits(other.timestampSeconds)) return false; - if (multiTagResult == null) { - if (other.multiTagResult != null) return false; - } else if (!multiTagResult.equals(other.multiTagResult)) return false; - return true; - } - - @Override - public String toString() { - return "PhotonPipelineResult [targets=" - + targets - + ", latencyMillis=" - + latencyMillis - + ", timestampSeconds=" - + timestampSeconds - + ", multiTagResult=" - + multiTagResult - + "]"; - } -} +// /* +// * Copyright (C) Photon Vision. +// * +// * This program is free software: you can redistribute it and/or modify +// * it under the terms of the GNU General Public License as published by +// * the Free Software Foundation, either version 3 of the License, or +// * (at your option) any later version. +// * +// * This program is distributed in the hope that it will be useful, +// * but WITHOUT ANY WARRANTY; without even the implied warranty of +// * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// * GNU General Public License for more details. +// * +// * You should have received a copy of the GNU General Public License +// * along with this program. If not, see . +// */ + +// package org.photonvision.targeting; + +// import java.util.ArrayList; +// import java.util.List; +// import org.photonvision.common.dataflow.structures.Packet; + +// import org.photonvision.proto.PhotonTypes; + +// /** 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<>(); + +// // 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. +// * +// * @param latencyMillis The latency in the pipeline. +// * @param targets The list of targets identified by the pipeline. +// * @param result Result from multi-target PNP. +// */ +// public PhotonPipelineResult( +// double latencyMillis, List targets, MultiTargetPNPResults result) { +// this.latencyMillis = latencyMillis; +// this.targets.addAll(targets); +// this.multiTagResult = result; +// } + +// /** +// * Returns the size of the packet needed to store this pipeline result. +// * +// * @return The size of the packet needed to store this pipeline result. +// */ +// public int getPacketSize() { +// return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES +// + 8 // 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. +// * +// * @return The best target of the pipeline result. +// */ +// public PhotonTrackedTarget getBestTarget() { +// if (!hasTargets() && !HAS_WARNED) { +// String errStr = +// "This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() " +// + "before calling this method. For more information, please review the PhotonLib " +// + "documentation at http://docs.photonvision.org"; +// System.err.println(errStr); +// new Exception().printStackTrace(); +// HAS_WARNED = true; +// } +// return hasTargets() ? targets.get(0) : null; +// } + +// /** +// * Returns the latency in the pipeline. +// * +// * @return The latency in the pipeline. +// */ +// public double getLatencyMillis() { +// return latencyMillis; +// } + +// /** +// * Returns the estimated time the frame was taken, This is more accurate than using +// * getLatencyMillis() +// * +// * @return The timestamp in seconds, or -1 if this result has no timestamp set. +// */ +// 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.size() > 0; +// } + +// /** +// * Returns a copy of the vector of targets. +// * +// * @return A copy of the vector of targets. +// */ +// public List getTargets() { +// return new ArrayList<>(targets); +// } + +// /** +// * Return the latest mulit-target result. Be sure to check +// * getMultiTagResult().estimatedPose.isPresent before using the pose estimate! +// */ +// public MultiTargetPNPResults getMultiTagResult() { +// return multiTagResult; +// } + +// /** +// * Populates the fields of the pipeline result from the packet. +// * +// * @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; +// int result = 1; +// result = prime * result + ((targets == null) ? 0 : targets.hashCode()); +// long temp; +// temp = Double.doubleToLongBits(latencyMillis); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// temp = Double.doubleToLongBits(timestampSeconds); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode()); +// return result; +// } + +// @Override +// public boolean equals(Object obj) { +// if (this == obj) return true; +// if (obj == null) return false; +// if (getClass() != obj.getClass()) return false; +// PhotonPipelineResult other = (PhotonPipelineResult) obj; +// if (targets == null) { +// if (other.targets != null) return false; +// } else if (!targets.equals(other.targets)) return false; +// if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) +// return false; +// if (Double.doubleToLongBits(timestampSeconds) +// != Double.doubleToLongBits(other.timestampSeconds)) return false; +// if (multiTagResult == null) { +// if (other.multiTagResult != null) return false; +// } else if (!multiTagResult.equals(other.multiTagResult)) return false; +// return true; +// } + +// @Override +// public String toString() { +// return "PhotonPipelineResult [targets=" +// + targets +// + ", latencyMillis=" +// + latencyMillis +// + ", timestampSeconds=" +// + timestampSeconds +// + ", multiTagResult=" +// + multiTagResult +// + "]"; +// } +// } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java new file mode 100644 index 0000000000..9c13b951a9 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java @@ -0,0 +1,36 @@ +package org.photonvision.targeting; + +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult; + +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class PhotonPipelineResultProto implements Protobuf { + + @Override + public Class getTypeClass() { + return PhotonPipelineResult.class; + } + + @Override + public Descriptor getDescriptor() { + return PhotonPipelineResult.getDescriptor(); + } + + @Override + public PhotonPipelineResult createMessage() { + return PhotonPipelineResult.newInstance(); + } + + @Override + public PhotonPipelineResult unpack(PhotonPipelineResult msg) { + return msg; + } + + @Override + public void pack(PhotonPipelineResult msg, PhotonPipelineResult value) { + msg.copyFrom(value); + } + + public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto(); +} 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 40263f5cc5..08a5571ef7 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -1,293 +1,293 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -package org.photonvision.targeting; - -import edu.wpi.first.math.geometry.Transform3d; -import java.util.ArrayList; -import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.utils.PacketUtils; - -public class PhotonTrackedTarget { - private static final int MAX_CORNERS = 8; - 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 List minAreaRectCorners; - - // Corners from whatever corner detection method was used - private List detectedCorners; - - public PhotonTrackedTarget() {} - - /** Construct a tracked target, given exactly 4 corners */ - public PhotonTrackedTarget( - double yaw, - double pitch, - double area, - double skew, - int id, - Transform3d pose, - Transform3d altPose, - double ambiguity, - List minAreaRectCorners, - List detectedCorners) { - assert minAreaRectCorners.size() == 4; - - if (detectedCorners.size() > MAX_CORNERS) { - detectedCorners = detectedCorners.subList(0, MAX_CORNERS); - } - - this.yaw = yaw; - this.pitch = pitch; - this.area = area; - this.skew = skew; - this.fiducialId = id; - this.bestCameraToTarget = pose; - this.altCameraToTarget = altPose; - this.minAreaRectCorners = minAreaRectCorners; - this.detectedCorners = detectedCorners; - this.poseAmbiguity = ambiguity; - } - - public double getYaw() { - return yaw; - } - - public double getPitch() { - return pitch; - } - - public double getArea() { - return area; - } - - public double getSkew() { - return skew; - } - - /** Get the Fiducial ID, or -1 if not set. */ - public int getFiducialId() { - return fiducialId; - } - - /** - * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be - * ambiguous. -1 if invalid. - */ - public double getPoseAmbiguity() { - return poseAmbiguity; - } - - /** - * Return a list of the 4 corners in image space (origin top left, x right, y down), in no - * particular order, of the minimum area bounding rectangle of this target - */ - public List getMinAreaRectCorners() { - return minAreaRectCorners; - } - - /** - * Return a list of the n corners in image space (origin top left, x right, y down), in no - * particular order, detected for this target. - * - *

For fiducials, the order is known and is always counter-clock wise around the tag, like so: - * - * 

​
-     * ⟶ +X  3 ----- 2
-     * |      |       |
-     * V      |       |
-     * +Y     0 ----- 1
-     *
- */ - public List getDetectedCorners() { - return detectedCorners; - } - - /** - * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag - * space (X forward, Y left, Z up) with the lowest reprojection error - */ - public Transform3d getBestCameraToTarget() { - return bestCameraToTarget; - } - - /** - * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag - * space (X forward, Y left, Z up) with the highest reprojection error - */ - public Transform3d getAlternateCameraToTarget() { - return altCameraToTarget; - } - - @Override - public int hashCode() { - final int prime = 31; - int result = 1; - long temp; - temp = Double.doubleToLongBits(yaw); - result = prime * result + (int) (temp ^ (temp >>> 32)); - temp = Double.doubleToLongBits(pitch); - result = prime * result + (int) (temp ^ (temp >>> 32)); - temp = Double.doubleToLongBits(area); - result = prime * result + (int) (temp ^ (temp >>> 32)); - temp = Double.doubleToLongBits(skew); - result = prime * result + (int) (temp ^ (temp >>> 32)); - result = prime * result + fiducialId; - result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode()); - result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode()); - temp = Double.doubleToLongBits(poseAmbiguity); - result = prime * result + (int) (temp ^ (temp >>> 32)); - result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode()); - result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.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; - PhotonTrackedTarget other = (PhotonTrackedTarget) obj; - if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false; - if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false; - if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false; - if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false; - if (fiducialId != other.fiducialId) return false; - if (bestCameraToTarget == null) { - if (other.bestCameraToTarget != null) return false; - } else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false; - if (altCameraToTarget == null) { - if (other.altCameraToTarget != null) return false; - } else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false; - if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity)) - return false; - if (minAreaRectCorners == null) { - if (other.minAreaRectCorners != null) return false; - } else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false; - if (detectedCorners == null) { - if (other.detectedCorners != null) return false; - } else if (!detectedCorners.equals(other.detectedCorners)) return false; - 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{" - + "yaw=" - + yaw - + ", pitch=" - + pitch - + ", area=" - + area - + ", skew=" - + skew - + ", fiducialId=" - + fiducialId - + ", cameraToTarget=" - + bestCameraToTarget - + ", targetCorners=" - + minAreaRectCorners - + '}'; - } -} +// /* +// * Copyright (C) Photon Vision. +// * +// * This program is free software: you can redistribute it and/or modify +// * it under the terms of the GNU General Public License as published by +// * the Free Software Foundation, either version 3 of the License, or +// * (at your option) any later version. +// * +// * This program is distributed in the hope that it will be useful, +// * but WITHOUT ANY WARRANTY; without even the implied warranty of +// * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// * GNU General Public License for more details. +// * +// * You should have received a copy of the GNU General Public License +// * along with this program. If not, see . +// */ + +// package org.photonvision.targeting; + +// import edu.wpi.first.math.geometry.Transform3d; +// import java.util.ArrayList; +// import java.util.List; +// import org.photonvision.common.dataflow.structures.Packet; +// import org.photonvision.utils.PacketUtils; + +// public class PhotonTrackedTarget { +// private static final int MAX_CORNERS = 8; +// 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 List minAreaRectCorners; + +// // Corners from whatever corner detection method was used +// private List detectedCorners; + +// public PhotonTrackedTarget() {} + +// /** Construct a tracked target, given exactly 4 corners */ +// public PhotonTrackedTarget( +// double yaw, +// double pitch, +// double area, +// double skew, +// int id, +// Transform3d pose, +// Transform3d altPose, +// double ambiguity, +// List minAreaRectCorners, +// List detectedCorners) { +// assert minAreaRectCorners.size() == 4; + +// if (detectedCorners.size() > MAX_CORNERS) { +// detectedCorners = detectedCorners.subList(0, MAX_CORNERS); +// } + +// this.yaw = yaw; +// this.pitch = pitch; +// this.area = area; +// this.skew = skew; +// this.fiducialId = id; +// this.bestCameraToTarget = pose; +// this.altCameraToTarget = altPose; +// this.minAreaRectCorners = minAreaRectCorners; +// this.detectedCorners = detectedCorners; +// this.poseAmbiguity = ambiguity; +// } + +// public double getYaw() { +// return yaw; +// } + +// public double getPitch() { +// return pitch; +// } + +// public double getArea() { +// return area; +// } + +// public double getSkew() { +// return skew; +// } + +// /** Get the Fiducial ID, or -1 if not set. */ +// public int getFiducialId() { +// return fiducialId; +// } + +// /** +// * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be +// * ambiguous. -1 if invalid. +// */ +// public double getPoseAmbiguity() { +// return poseAmbiguity; +// } + +// /** +// * Return a list of the 4 corners in image space (origin top left, x right, y down), in no +// * particular order, of the minimum area bounding rectangle of this target +// */ +// public List getMinAreaRectCorners() { +// return minAreaRectCorners; +// } + +// /** +// * Return a list of the n corners in image space (origin top left, x right, y down), in no +// * particular order, detected for this target. +// * +// *

For fiducials, the order is known and is always counter-clock wise around the tag, like so: +// * +// * 

​
+//      * ⟶ +X  3 ----- 2
+//      * |      |       |
+//      * V      |       |
+//      * +Y     0 ----- 1
+//      *
+// */ +// public List getDetectedCorners() { +// return detectedCorners; +// } + +// /** +// * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag +// * space (X forward, Y left, Z up) with the lowest reprojection error +// */ +// public Transform3d getBestCameraToTarget() { +// return bestCameraToTarget; +// } + +// /** +// * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag +// * space (X forward, Y left, Z up) with the highest reprojection error +// */ +// public Transform3d getAlternateCameraToTarget() { +// return altCameraToTarget; +// } + +// @Override +// public int hashCode() { +// final int prime = 31; +// int result = 1; +// long temp; +// temp = Double.doubleToLongBits(yaw); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// temp = Double.doubleToLongBits(pitch); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// temp = Double.doubleToLongBits(area); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// temp = Double.doubleToLongBits(skew); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// result = prime * result + fiducialId; +// result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode()); +// result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode()); +// temp = Double.doubleToLongBits(poseAmbiguity); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode()); +// result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.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; +// PhotonTrackedTarget other = (PhotonTrackedTarget) obj; +// if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false; +// if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false; +// if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false; +// if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false; +// if (fiducialId != other.fiducialId) return false; +// if (bestCameraToTarget == null) { +// if (other.bestCameraToTarget != null) return false; +// } else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false; +// if (altCameraToTarget == null) { +// if (other.altCameraToTarget != null) return false; +// } else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false; +// if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity)) +// return false; +// if (minAreaRectCorners == null) { +// if (other.minAreaRectCorners != null) return false; +// } else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false; +// if (detectedCorners == null) { +// if (other.detectedCorners != null) return false; +// } else if (!detectedCorners.equals(other.detectedCorners)) return false; +// 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{" +// + "yaw=" +// + yaw +// + ", pitch=" +// + pitch +// + ", area=" +// + area +// + ", skew=" +// + skew +// + ", fiducialId=" +// + fiducialId +// + ", cameraToTarget=" +// + bestCameraToTarget +// + ", targetCorners=" +// + minAreaRectCorners +// + '}'; +// } +// } 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 9808079a5d..36eb04b258 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -1,52 +1,52 @@ -/* - * 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 . - */ +// /* +// * Copyright (C) Photon Vision. +// * +// * This program is free software: you can redistribute it and/or modify +// * it under the terms of the GNU General Public License as published by +// * the Free Software Foundation, either version 3 of the License, or +// * (at your option) any later version. +// * +// * This program is distributed in the hope that it will be useful, +// * but WITHOUT ANY WARRANTY; without even the implied warranty of +// * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// * GNU General Public License for more details. +// * +// * You should have received a copy of the GNU General Public License +// * along with this program. If not, see . +// */ -package org.photonvision.targeting; +// package org.photonvision.targeting; -import java.util.Objects; +// import java.util.Objects; -/** - * Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels. - * Origin at the top left, plus-x to the right, plus-y down. - */ -public class TargetCorner { - public final double x; - public final double y; +// /** +// * Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels. +// * Origin at the top left, plus-x to the right, plus-y down. +// */ +// public class TargetCorner { +// public final double x; +// public final double y; - public TargetCorner(double cx, double cy) { - this.x = cx; - this.y = cy; - } +// public TargetCorner(double cx, double cy) { +// this.x = cx; +// this.y = cy; +// } - @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - TargetCorner that = (TargetCorner) o; - return Double.compare(that.x, x) == 0 && Double.compare(that.y, y) == 0; - } +// @Override +// public boolean equals(Object o) { +// if (this == o) return true; +// if (o == null || getClass() != o.getClass()) return false; +// TargetCorner that = (TargetCorner) o; +// return Double.compare(that.x, x) == 0 && Double.compare(that.y, y) == 0; +// } - @Override - public int hashCode() { - return Objects.hash(x, y); - } +// @Override +// public int hashCode() { +// return Objects.hash(x, y); +// } - @Override - public String toString() { - return "(" + x + "," + y + ')'; - } -} +// @Override +// public String toString() { +// return "(" + x + "," + y + ')'; +// } +// } diff --git a/photon-targeting/src/main/proto/photon_types.proto b/photon-targeting/src/main/proto/photon_types.proto new file mode 100644 index 0000000000..b328bed18e --- /dev/null +++ b/photon-targeting/src/main/proto/photon_types.proto @@ -0,0 +1,44 @@ +syntax = "proto3"; + +package photonvision.proto; + +option java_package = "org.photonvision.proto"; + +import "geometry3d.proto"; + +message PhotonPipelineResult { + double latency_ms = 1; + double timestamp_sec = 2; + + message TargetCorner { + double x = 1; + double y = 2; + } + + message PhotonTrackedTarget { + double yaw = 1; + double pitch = 2; + double area = 3; + double skew = 4; + int32 fiducialId = 5; + wpi.proto.ProtobufTransform3d bestCameraToTarget = 6; + wpi.proto.ProtobufTransform3d altCameraToTarget = 7; + double poseAmbiguity = 8; + repeated TargetCorner minAreaRectCorners = 9; + repeated TargetCorner detectedCorners = 10; + } + + message MultiTargetPNPResults { + wpi.proto.ProtobufTransform3d best = 1; + double bestReprojErr = 2; + optional wpi.proto.ProtobufTransform3d alt = 3; + optional double altReprojErr = 4; + double ambiguity = 5; + + repeated int32 fiducial_ids_used = 6; + } + + repeated PhotonTrackedTarget targets = 3; + MultiTargetPNPResults multi_target_result = 4; +} +