From d1bf4f99f2d40f6b21670bdacc123ef7f6f09c38 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 14 Nov 2023 17:28:28 -0500 Subject: [PATCH 1/9] Make MultiTagPNPResult and PNPResult singular --- .../vision/pipe/impl/MultiTargetPNPPipe.java | 14 ++++++------- .../vision/pipeline/AprilTagPipeline.java | 4 ++-- .../vision/pipeline/ArucoPipeline.java | 4 ++-- .../vision/pipeline/Calibrate3dPipeline.java | 4 ++-- .../pipeline/result/CVPipelineResult.java | 14 ++++++------- .../org/photonvision/PhotonPoseEstimator.java | 6 +++--- .../simulation/PhotonCameraSim.java | 14 ++++++------- .../simulation/SimPhotonCamera.java | 4 ++-- .../java/org/photonvision/PacketTest.java | 8 ++++---- .../photonvision/estimation/OpenCVHelp.java | 16 +++++++-------- .../estimation/VisionEstimation.java | 18 ++++++++--------- ...Results.java => MultiTargetPNPResult.java} | 20 +++++++++---------- .../{PNPResults.java => PNPResult.java} | 18 ++++++++--------- .../targeting/PhotonPipelineResult.java | 10 +++++----- 14 files changed, 77 insertions(+), 77 deletions(-) rename photon-targeting/src/main/java/org/photonvision/targeting/{MultiTargetPNPResults.java => MultiTargetPNPResult.java} (81%) rename photon-targeting/src/main/java/org/photonvision/targeting/{PNPResults.java => PNPResult.java} (93%) 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 313d8a19e0..4a656aeb43 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 @@ -24,7 +24,7 @@ import org.photonvision.common.logging.Logger; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.pipe.CVPipe; import org.photonvision.vision.target.TrackedTarget; @@ -32,13 +32,13 @@ /** Estimate the camera pose given multiple Apriltag observations */ public class MultiTargetPNPPipe extends CVPipe< - List, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> { + List, MultiTargetPNPResult, MultiTargetPNPPipe.MultiTargetPNPPipeParams> { private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule); private boolean hasWarned = false; @Override - protected MultiTargetPNPResults process(List targetList) { + protected MultiTargetPNPResult process(List targetList) { if (params == null || params.cameraCoefficients == null || params.cameraCoefficients.getCameraIntrinsicsMat() == null @@ -48,13 +48,13 @@ protected MultiTargetPNPResults process(List targetList) { "Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution..."); hasWarned = true; } - return new MultiTargetPNPResults(); + return new MultiTargetPNPResult(); } return calculateCameraInField(targetList); } - private MultiTargetPNPResults calculateCameraInField(List targetList) { + private MultiTargetPNPResult calculateCameraInField(List targetList) { // Find tag IDs that exist in the tag layout var tagIDsUsed = new ArrayList(); for (var target : targetList) { @@ -64,7 +64,7 @@ private MultiTargetPNPResults calculateCameraInField(List targetL // Only run with multiple targets if (tagIDsUsed.size() < 2) { - return new MultiTargetPNPResults(); + return new MultiTargetPNPResult(); } var estimatedPose = @@ -75,7 +75,7 @@ private MultiTargetPNPResults calculateCameraInField(List targetL params.atfl, params.targetModel); - return new MultiTargetPNPResults(estimatedPose, tagIDsUsed); + return new MultiTargetPNPResult(estimatedPose, tagIDsUsed); } public static class MultiTargetPNPPipeParams { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index 1eb040616a..5d2abe1a07 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -31,7 +31,7 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.util.math.MathUtils; import org.photonvision.estimation.TargetModel; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.apriltag.AprilTagFamily; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -149,7 +149,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting } // Do multi-tag pose estimation - MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); + MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult(); if (settings.solvePNPEnabled && settings.doMultiTarget) { var multiTagOutput = multiTagPNPPipe.run(targetList); sumPipeNanosElapsed += multiTagOutput.nanosElapsed; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java index c463cb14e4..727eee69ff 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java @@ -47,7 +47,7 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.util.math.MathUtils; import org.photonvision.estimation.TargetModel; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.aruco.ArucoDetectionResult; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -170,7 +170,7 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) } // Do multi-tag pose estimation - MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); + MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult(); if (settings.solvePNPEnabled && settings.doMultiTarget) { var multiTagOutput = multiTagPNPPipe.run(targetList); sumPipeNanosElapsed += multiTagOutput.nanosElapsed; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java index 95008e77f9..e1a3bc8aae 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java @@ -34,7 +34,7 @@ import org.photonvision.common.logging.Logger; import org.photonvision.common.util.SerializationUtils; import org.photonvision.common.util.file.FileUtils; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -147,7 +147,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se sumPipeNanosElapsed, fps, // Unused but here in case Collections.emptyList(), - new MultiTargetPNPResults(), + new MultiTargetPNPResult(), new Frame( new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties)); } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java index 5993cc9531..4186fd91e5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java @@ -20,7 +20,7 @@ import java.util.Collections; import java.util.List; import org.photonvision.common.util.math.MathUtils; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.target.TrackedTarget; @@ -31,23 +31,23 @@ public class CVPipelineResult implements Releasable { public final double fps; public final List targets; public final Frame inputAndOutputFrame; - public MultiTargetPNPResults multiTagResult; + public MultiTargetPNPResult multiTagResult; public CVPipelineResult( double processingNanos, double fps, List targets, Frame inputFrame) { - this(processingNanos, fps, targets, new MultiTargetPNPResults(), inputFrame); + this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame); } public CVPipelineResult( double processingNanos, double fps, List targets, - MultiTargetPNPResults multiTagResults, + MultiTargetPNPResult multiTagResult, Frame inputFrame) { this.processingNanos = processingNanos; this.fps = fps; this.targets = targets != null ? targets : Collections.emptyList(); - this.multiTagResult = multiTagResults; + this.multiTagResult = multiTagResult; this.inputAndOutputFrame = inputFrame; } @@ -56,8 +56,8 @@ public CVPipelineResult( double processingNanos, double fps, List targets, - MultiTargetPNPResults multiTagResults) { - this(processingNanos, fps, targets, multiTagResults, null); + MultiTargetPNPResult multiTagResult) { + this(processingNanos, fps, targets, multiTagResult, null); } public boolean hasTargets() { diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 32b4c7ba1f..dbf36292c2 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -443,15 +443,15 @@ private Optional multiTagOnRioStrategy( return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); } - var pnpResults = + var pnpResult = VisionEstimation.estimateCamPosePNP( cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel); // try fallback strategy if solvePNP fails for some reason - if (!pnpResults.isPresent) + if (!pnpResult.isPresent) return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); var best = new Pose3d() - .plus(pnpResults.best) // field-to-camera + .plus(pnpResult.best) // field-to-camera .plus(robotToCamera.inverse()); // field-to-robot return Optional.of( 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 f725837e13..f125ea2ac3 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -54,8 +54,8 @@ import org.photonvision.estimation.RotTrlTransform3d; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.MultiTargetPNPResults; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PNPResult; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -419,7 +419,7 @@ public PhotonPipelineResult process( // projected target can't be detected, skip to next if (!(canSeeCorners(noisyTargetCorners) && areaPercent >= minTargetAreaPercent)) continue; - var pnpSim = new PNPResults(); + var pnpSim = new PNPResult(); if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP pnpSim = OpenCVHelp.solvePNP_SQUARE( @@ -516,21 +516,21 @@ public PhotonPipelineResult process( } else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose); // calculate multitag results - var multitagResults = new MultiTargetPNPResults(); + var multitagResult = new MultiTargetPNPResult(); // TODO: Implement ATFL subscribing in backend // var tagLayout = cam.getAprilTagFieldLayout(); var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout); if (visibleLayoutTags.size() > 1) { List usedIDs = visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList()); - var pnpResults = + var pnpResult = VisionEstimation.estimateCamPosePNP( prop.getIntrinsics(), prop.getDistCoeffs(), detectableTgts, tagLayout, TargetModel.kAprilTag16h5); - multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs); + multitagResult = new MultiTargetPNPResult(pnpResult, usedIDs); } // sort target order @@ -539,7 +539,7 @@ public PhotonPipelineResult process( } // put this simulated data to NT - return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResults); + return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResult); } /** diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java index 58e3e2fcf7..5eb73b35eb 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java @@ -35,7 +35,7 @@ import org.photonvision.PhotonTargetSortMode; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.networktables.NTTopicSet; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -142,7 +142,7 @@ public void submitProcessedFrame( } PhotonPipelineResult newResult = - new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults()); + new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResult()); var newPacket = new Packet(newResult.getPacketSize()); newResult.populatePacket(newPacket); ts.rawBytesEntry.set(newPacket.getData()); diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java index c29a629b5b..a2f98a27c2 100644 --- a/photon-lib/src/test/java/org/photonvision/PacketTest.java +++ b/photon-lib/src/test/java/org/photonvision/PacketTest.java @@ -30,8 +30,8 @@ import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.Test; import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.targeting.MultiTargetPNPResults; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PNPResult; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; @@ -174,8 +174,8 @@ public void testMultiTargetSerde() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8)))), - new MultiTargetPNPResults( - new PNPResults( + new MultiTargetPNPResult( + new PNPResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), List.of(1, 2, 3))); 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 84e5d8d7cc..31b22a7cc7 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -45,7 +45,7 @@ import org.opencv.core.Rect; import org.opencv.core.RotatedRect; import org.opencv.imgproc.Imgproc; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.PNPResult; import org.photonvision.targeting.TargetCorner; public final class OpenCVHelp { @@ -401,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 PNPResults solvePNP_SQUARE( + public static PNPResult solvePNP_SQUARE( Matrix cameraMatrix, Matrix distCoeffs, List modelTrls, @@ -467,14 +467,14 @@ public static PNPResults solvePNP_SQUARE( if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result"); if (alt != null) - return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]); - else return new PNPResults(best, errors[0]); + return new PNPResult(best, alt, errors[0] / errors[1], errors[0], errors[1]); + else return new PNPResult(best, errors[0]); } // solvePnP failed catch (Exception e) { System.err.println("SolvePNP_SQUARE failed!"); e.printStackTrace(); - return new PNPResults(); + return new PNPResult(); } finally { // release our Mats from native memory objectMat.release(); @@ -509,7 +509,7 @@ public static PNPResults solvePNP_SQUARE( * model points are supplied relative to the origin, this transformation brings the camera to * the origin. */ - public static PNPResults solvePNP_SQPNP( + public static PNPResult solvePNP_SQPNP( Matrix cameraMatrix, Matrix distCoeffs, List objectTrls, @@ -558,11 +558,11 @@ public static PNPResults solvePNP_SQPNP( // check if solvePnP failed with NaN results if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result"); - return new PNPResults(best, error[0]); + return new PNPResult(best, error[0]); } catch (Exception e) { System.err.println("SolvePNP_SQPNP failed!"); e.printStackTrace(); - return new PNPResults(); + return new PNPResult(); } } } 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 9ad6e0fc4e..f163a60a04 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -29,7 +29,7 @@ import java.util.Objects; import java.util.stream.Collectors; import org.opencv.core.Point; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.PNPResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; @@ -64,9 +64,9 @@ public static List getVisibleLayoutTags( * @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded. * @param tagLayout The known tag layout on the field * @return The transformation that maps the field origin to the camera pose. Ensure the {@link - * PNPResults} are present before utilizing them. + * PNPResult} are present before utilizing them. */ - public static PNPResults estimateCamPosePNP( + public static PNPResult estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, List visTags, @@ -76,7 +76,7 @@ public static PNPResults estimateCamPosePNP( || visTags == null || tagLayout.getTags().size() == 0 || visTags.size() == 0) { - return new PNPResults(); + return new PNPResult(); } var corners = new ArrayList(); @@ -93,7 +93,7 @@ public static PNPResults estimateCamPosePNP( }); } if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { - return new PNPResults(); + return new PNPResult(); } Point[] points = OpenCVHelp.cornersToPoints(corners); @@ -101,14 +101,14 @@ public static PNPResults estimateCamPosePNP( if (knownTags.size() == 1) { var camToTag = OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points); - if (!camToTag.isPresent) return new PNPResults(); + if (!camToTag.isPresent) return new PNPResult(); 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 new PNPResults( + return new PNPResult( new Transform3d(o, bestPose), new Transform3d(o, altPose), camToTag.ambiguity, @@ -120,8 +120,8 @@ public static PNPResults estimateCamPosePNP( var objectTrls = new ArrayList(); for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose)); var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points); - if (!camToOrigin.isPresent) return new PNPResults(); - return new PNPResults( + if (!camToOrigin.isPresent) return new PNPResult(); + return new PNPResult( camToOrigin.best.inverse(), camToOrigin.alt.inverse(), camToOrigin.ambiguity, diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java similarity index 81% rename from photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java rename to photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java index 453c0a57e5..4792c3fb72 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java @@ -21,30 +21,30 @@ import java.util.List; import org.photonvision.common.dataflow.structures.Packet; -public class MultiTargetPNPResults { +public class MultiTargetPNPResult { // 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 static final int PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS); - public PNPResults estimatedPose = new PNPResults(); + public PNPResult estimatedPose = new PNPResult(); public List fiducialIDsUsed = List.of(); - public MultiTargetPNPResults() {} + public MultiTargetPNPResult() {} - public MultiTargetPNPResults(PNPResults results, List ids) { + public MultiTargetPNPResult(PNPResult results, List ids) { estimatedPose = results; fiducialIDsUsed = ids; } - public static MultiTargetPNPResults createFromPacket(Packet packet) { - var results = PNPResults.createFromPacket(packet); + public static MultiTargetPNPResult createFromPacket(Packet packet) { + var results = PNPResult.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); + return new MultiTargetPNPResult(results, ids); } public void populatePacket(Packet packet) { @@ -72,7 +72,7 @@ public boolean equals(Object obj) { if (this == obj) return true; if (obj == null) return false; if (getClass() != obj.getClass()) return false; - MultiTargetPNPResults other = (MultiTargetPNPResults) obj; + MultiTargetPNPResult other = (MultiTargetPNPResult) obj; if (estimatedPose == null) { if (other.estimatedPose != null) return false; } else if (!estimatedPose.equals(other.estimatedPose)) return false; @@ -84,7 +84,7 @@ public boolean equals(Object obj) { @Override public String toString() { - return "MultiTargetPNPResults [estimatedPose=" + return "MultiTargetPNPResult [estimatedPose=" + estimatedPose + ", fiducialIDsUsed=" + fiducialIDsUsed diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java similarity index 93% rename from photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java rename to photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java index 11a77547fc..edb4c0fc49 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java @@ -30,7 +30,7 @@ *

Note that the coordinate frame of these transforms depends on the implementing solvePnP * method. */ -public class PNPResults { +public class PNPResult { /** * If this result is valid. A false value indicates there was an error in estimation, and this * result should not be used. @@ -59,7 +59,7 @@ public class PNPResults { public final double ambiguity; /** An empty (invalid) result. */ - public PNPResults() { + public PNPResult() { this.isPresent = false; this.best = new Transform3d(); this.alt = new Transform3d(); @@ -68,11 +68,11 @@ public PNPResults() { this.altReprojErr = 0; } - public PNPResults(Transform3d best, double bestReprojErr) { + public PNPResult(Transform3d best, double bestReprojErr) { this(best, best, 0, bestReprojErr, bestReprojErr); } - public PNPResults( + public PNPResult( Transform3d best, Transform3d alt, double ambiguity, @@ -88,7 +88,7 @@ public PNPResults( public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3); - public static PNPResults createFromPacket(Packet packet) { + public static PNPResult createFromPacket(Packet packet) { var present = packet.decodeBoolean(); var best = PacketUtils.decodeTransform(packet); var alt = PacketUtils.decodeTransform(packet); @@ -96,9 +96,9 @@ public static PNPResults createFromPacket(Packet packet) { var altEr = packet.decodeDouble(); var ambiguity = packet.decodeDouble(); if (present) { - return new PNPResults(best, alt, ambiguity, bestEr, altEr); + return new PNPResult(best, alt, ambiguity, bestEr, altEr); } else { - return new PNPResults(); + return new PNPResult(); } } @@ -134,7 +134,7 @@ public boolean equals(Object obj) { if (this == obj) return true; if (obj == null) return false; if (getClass() != obj.getClass()) return false; - PNPResults other = (PNPResults) obj; + PNPResult other = (PNPResult) obj; if (isPresent != other.isPresent) return false; if (best == null) { if (other.best != null) return false; @@ -153,7 +153,7 @@ public boolean equals(Object obj) { @Override public String toString() { - return "PNPResults [isPresent=" + return "PNPResult [isPresent=" + isPresent + ", best=" + best 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..b0910ea253 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -35,7 +35,7 @@ public class PhotonPipelineResult { private double timestampSeconds = -1; // Multi-tag result - private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); + private MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult(); /** Constructs an empty pipeline result. */ public PhotonPipelineResult() {} @@ -59,7 +59,7 @@ public PhotonPipelineResult(double latencyMillis, List targ * @param result Result from multi-target PNP. */ public PhotonPipelineResult( - double latencyMillis, List targets, MultiTargetPNPResults result) { + double latencyMillis, List targets, MultiTargetPNPResult result) { this.latencyMillis = latencyMillis; this.targets.addAll(targets); this.multiTagResult = result; @@ -73,7 +73,7 @@ public PhotonPipelineResult( public int getPacketSize() { return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES + 8 // latency - + MultiTargetPNPResults.PACK_SIZE_BYTES + + MultiTargetPNPResult.PACK_SIZE_BYTES + 1; // target count } @@ -146,7 +146,7 @@ public List getTargets() { * Return the latest mulit-target result. Be sure to check * getMultiTagResult().estimatedPose.isPresent before using the pose estimate! */ - public MultiTargetPNPResults getMultiTagResult() { + public MultiTargetPNPResult getMultiTagResult() { return multiTagResult; } @@ -159,7 +159,7 @@ public MultiTargetPNPResults getMultiTagResult() { public Packet createFromPacket(Packet packet) { // Decode latency, existence of targets, and number of targets. latencyMillis = packet.decodeDouble(); - this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet); + this.multiTagResult = MultiTargetPNPResult.createFromPacket(packet); byte targetCount = packet.decodeByte(); targets.clear(); From b495cf3b9e5bcc861d0ce82901916cdd8ab6a661 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 14 Nov 2023 17:34:38 -0500 Subject: [PATCH 2/9] add java tests --- .../targeting/MultiTargetPNPResultTest.java | 66 +++ .../photonvision/targeting/PNPResultTest.java | 77 ++++ .../targeting/PhotonPipelineResultTest.java | 399 ++++++++++++++++++ .../targeting/PhotonTrackedTargetTest.java | 119 ++++++ .../targeting/TargetCornerTest.java | 41 ++ 5 files changed, 702 insertions(+) create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java new file mode 100644 index 0000000000..4e28237ac5 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java @@ -0,0 +1,66 @@ +/* + * 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 static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class MultiTargetPNPResultTest { + @Test + public void equalityTest() { + var a = new MultiTargetPNPResult(); + var b = new MultiTargetPNPResult(); + assertEquals(a, b); + + a = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + + b = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 8, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(3, 4, 7)); + var b = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + + assertNotEquals(a, b); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java new file mode 100644 index 0000000000..9f86473eaa --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java @@ -0,0 +1,77 @@ +/* + * 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 static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import org.junit.jupiter.api.Test; + +public class PNPResultTest { + @Test + public void equalityTest() { + var a = new PNPResult(); + var b = new PNPResult(); + assertEquals(a, b); + + a = new PNPResult(new Transform3d(0, 1, 2, new Rotation3d()), 0.0); + b = new PNPResult(new Transform3d(0, 1, 2, new Rotation3d()), 0.0); + assertEquals(a, b); + + a = + new PNPResult( + new Transform3d(0, 1, 2, new Rotation3d()), + new Transform3d(3, 4, 5, new Rotation3d()), + 0.5, + 0.1, + 0.1); + b = + new PNPResult( + new Transform3d(0, 1, 2, new Rotation3d()), + new Transform3d(3, 4, 5, new Rotation3d()), + 0.5, + 0.1, + 0.1); + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = new PNPResult(new Transform3d(0, 1, 2, new Rotation3d()), 0.0); + var b = new PNPResult(new Transform3d(3, 4, 5, new Rotation3d()), 0.1); + assertNotEquals(a, b); + + a = + new PNPResult( + new Transform3d(3, 4, 5, new Rotation3d()), + new Transform3d(0, 1, 2, new Rotation3d()), + 0.5, + 0.1, + 0.1); + b = + new PNPResult( + new Transform3d(3, 4, 5, new Rotation3d()), + new Transform3d(0, 1, 2, new Rotation3d()), + 0.5, + 0.1, + 0.2); + assertNotEquals(a, b); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java new file mode 100644 index 0000000000..72dacda168 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -0,0 +1,399 @@ +/* + * 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 static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class PhotonPipelineResultTest { + @Test + public void equalityTest() { + var a = new PhotonPipelineResult(); + var b = new PhotonPipelineResult(); + assertEquals(a, b); + + a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + assertEquals(a, b); + + a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + var b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + assertNotEquals(a, b); + + a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 8, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(3, 4, 7))); + b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + assertNotEquals(a, b); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java new file mode 100644 index 0000000000..5d42a2b68f --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -0,0 +1,119 @@ +/* + * 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 static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class PhotonTrackedTargetTest { + @Test + public void equalityTest() { + var a = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + var b = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + var b = + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + assertNotEquals(a, b); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java new file mode 100644 index 0000000000..b813fb7afb --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java @@ -0,0 +1,41 @@ +/* + * 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 static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import org.junit.jupiter.api.Test; + +public class TargetCornerTest { + @Test + public void equalityTest() { + var a = new TargetCorner(0, 1); + var b = new TargetCorner(0, 1); + + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = new TargetCorner(0, 1); + var b = new TargetCorner(2, 4); + + assertNotEquals(a, b); + } +} From e764326d1f27b061b3eee8aec695af29a34ccf9f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 14 Nov 2023 17:34:57 -0500 Subject: [PATCH 3/9] Formatting fixes --- .../src/main/java/org/photonvision/estimation/OpenCVHelp.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 31b22a7cc7..8abdacb1ce 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -466,8 +466,7 @@ public static PNPResult 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 new PNPResult(best, alt, errors[0] / errors[1], errors[0], errors[1]); + if (alt != null) return new PNPResult(best, alt, errors[0] / errors[1], errors[0], errors[1]); else return new PNPResult(best, errors[0]); } // solvePnP failed From 28dedd2d6b18dbe0c1ec4ec7fa4625c52bfd4c72 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 14 Nov 2023 17:59:13 -0500 Subject: [PATCH 4/9] bring in the rest of the little stuff --- .../java/org/photonvision/PhotonCamera.java | 70 ++----------------- .../photonvision/estimation/OpenCVHelp.java | 4 +- .../photonvision/estimation/TargetModel.java | 2 +- .../estimation/VisionEstimation.java | 6 +- 4 files changed, 11 insertions(+), 71 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index cd64965990..3f201cfe33 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -24,8 +24,6 @@ package org.photonvision; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -38,12 +36,10 @@ import edu.wpi.first.networktables.IntegerEntry; import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.IntegerSubscriber; -import edu.wpi.first.networktables.MultiSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.RawSubscriber; -import edu.wpi.first.networktables.StringPublisher; import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -73,9 +69,8 @@ public class PhotonCamera implements AutoCloseable { IntegerPublisher pipelineIndexRequest, ledModeRequest; IntegerSubscriber pipelineIndexState, ledModeState; IntegerSubscriber heartbeatEntry; - private DoubleArraySubscriber cameraIntrinsicsSubscriber; - private DoubleArraySubscriber cameraDistortionSubscriber; - private StringPublisher atflPublisher; + DoubleArraySubscriber cameraIntrinsicsSubscriber; + DoubleArraySubscriber cameraDistortionSubscriber; @Override public void close() { @@ -99,7 +94,6 @@ public void close() { pipelineIndexRequest.close(); cameraIntrinsicsSubscriber.close(); cameraDistortionSubscriber.close(); - atflPublisher.close(); } private final String path; @@ -111,7 +105,7 @@ public void close() { private long prevHeartbeatValue = -1; private double prevHeartbeatChangeTime = 0; - private static final double HEARBEAT_DEBOUNCE_SEC = 0.5; + private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5; public static void setVersionCheckEnabled(boolean enabled) { VERSION_CHECK_ENABLED = enabled; @@ -119,12 +113,6 @@ public static void setVersionCheckEnabled(boolean enabled) { Packet packet = new Packet(1); - private static AprilTagFieldLayout lastSetTagLayout = - AprilTagFields.kDefaultField.loadAprilTagLayoutField(); - - // Existing is enough to make this multisubscriber do its thing - private final MultiSubscriber m_topicNameSubscriber; - /** * Constructs a PhotonCamera from a root table. * @@ -158,14 +146,6 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { ledModeRequest = photonvision_root_table.getIntegerTopic("ledModeRequest").publish(); ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1); versionEntry = photonvision_root_table.getStringTopic("version").subscribe(""); - - atflPublisher = photonvision_root_table.getStringTopic("apriltag_field_layout").publish(); - // Save the layout locally on Rio; on reboot, should be pushed out to NT clients - atflPublisher.getTopic().setPersistent(true); - - m_topicNameSubscriber = - new MultiSubscriber( - instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true)); } /** @@ -329,49 +309,9 @@ public boolean isConnected() { prevHeartbeatValue = curHeartbeat; } - return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC; + return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC; } - // TODO: Implement ATFL subscribing in backend - // /** - // * Get the last set {@link AprilTagFieldLayout}. The tag layout is shared between all - // PhotonVision - // * coprocessors on the same NT instance-- this method returns the most recent layout set. If no - // * layout has been set by the user, this is equal to {@link AprilTagFields#kDefaultField}. - // * - // * @return The last set layout - // */ - // public AprilTagFieldLayout getAprilTagFieldLayout() { - // return lastSetTagLayout; - // } - - // /** - // * Set the {@link AprilTagFieldLayout} used by all PhotonVision coprocessors that are (or might - // * later) connect to this robot. The topic is marked as persistent, so even if you only call - // this - // * once ever, it will be saved on the RoboRIO and pushed out to all NT clients when code - // reboots. - // * PhotonVision will also store a copy of this layout locally on the coprocessor, but - // subscribes - // * to this topic and the local copy will be updated when this function is called. - // * - // * @param layout The layout to use for *all* PhotonVision cameras - // * @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients - // * have updated their internal layouts. - // */ - // public boolean setAprilTagFieldLayout(AprilTagFieldLayout layout) { - // try { - // var layout_json = new ObjectMapper().writeValueAsString(layout); - // atflPublisher.set(layout_json); - // lastSetTagLayout = layout; - // return true; - // } catch (JsonProcessingException e) { - // MathSharedStore.reportError("Error setting ATFL in " + this.getName(), - // e.getStackTrace()); - // } - // return false; - // } - public Optional> getCameraMatrix() { var cameraMatrix = cameraIntrinsicsSubscriber.get(); if (cameraMatrix != null && cameraMatrix.length == 9) { @@ -428,7 +368,7 @@ else if (!isConnected()) { // Check for version. Warn if the versions aren't aligned. String versionString = versionEntry.get(""); - if (!versionString.equals("") && !PhotonVersion.versionMatches(versionString)) { + if (!versionString.isEmpty() && !PhotonVersion.versionMatches(versionString)) { // Error on a verified version mismatch // But stay silent otherwise DriverStation.reportWarning( 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 8abdacb1ce..196d6c845b 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -170,8 +170,8 @@ 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)); + for (Point point : points) { + corners.add(new TargetCorner(point.x, point.y)); } return corners; } diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java index f945d2c806..aa77a1e5d9 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java @@ -151,7 +151,7 @@ public TargetModel(List vertices) { */ public List getFieldVertices(Pose3d targetPose) { var basisChange = new RotTrlTransform3d(targetPose.getRotation(), targetPose.getTranslation()); - return vertices.stream().map(t -> basisChange.apply(t)).collect(Collectors.toList()); + return vertices.stream().map(basisChange::apply).collect(Collectors.toList()); } /** 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 f163a60a04..e1e270fc3c 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -74,8 +74,8 @@ public static PNPResult estimateCamPosePNP( TargetModel tagModel) { if (tagLayout == null || visTags == null - || tagLayout.getTags().size() == 0 - || visTags.size() == 0) { + || tagLayout.getTags().isEmpty() + || visTags.isEmpty()) { return new PNPResult(); } @@ -92,7 +92,7 @@ public static PNPResult estimateCamPosePNP( corners.addAll(tgt.getDetectedCorners()); }); } - if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { + if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) { return new PNPResult(); } Point[] points = OpenCVHelp.cornersToPoints(corners); From 34c880c6a8d05a87be800cc944bc1200d8c1c3e9 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 14 Nov 2023 18:40:07 -0500 Subject: [PATCH 5/9] final things --- photon-lib/build.gradle | 2 +- photon-lib/publish.gradle | 2 +- .../java/org/photonvision/PhotonUtils.java | 2 +- .../simulation/PhotonCameraSim.java | 2 +- .../simulation/SimCameraProperties.java | 15 +++++++-------- .../simulation/SimVisionSystem.java | 2 +- .../photonvision/simulation/VideoSimUtil.java | 18 ++++++++---------- .../simulation/VisionSystemSim.java | 5 ++--- .../photonvision/PhotonPoseEstimatorTest.java | 4 ++-- .../org/photonvision/PhotonVersionTest.java | 2 +- .../photonvision/estimation/OpenCVHelp.java | 6 +++--- .../targeting/MultiTargetPNPResult.java | 2 +- .../org/photonvision/targeting/PNPResult.java | 3 +-- .../targeting/PhotonPipelineResult.java | 10 ++++------ .../targeting/PhotonTrackedTarget.java | 6 +++--- 15 files changed, 37 insertions(+), 44 deletions(-) diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 02cb0fd064..2ff63fe4b7 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -46,7 +46,7 @@ dependencies { implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get() implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() - implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); + implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get() // Junit testImplementation wpilibTools.deps.wpilibJava("cscore") diff --git a/photon-lib/publish.gradle b/photon-lib/publish.gradle index ca67c43b0e..bc825b81aa 100644 --- a/photon-lib/publish.gradle +++ b/photon-lib/publish.gradle @@ -152,7 +152,7 @@ publishing { tasks.withType(PublishToMavenRepository) { doFirst { - println("Publishing to " + repository.url); + println("Publishing to " + repository.url) } } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonUtils.java b/photon-lib/src/main/java/org/photonvision/PhotonUtils.java index 4d35c5ac29..90c407f555 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonUtils.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonUtils.java @@ -201,7 +201,7 @@ public static Rotation2d getYawToPose(Pose2d robotPose, Pose2d targetPose) { * * @param robotPose Pose of the robot * @param targetPose Pose of the target - * @return + * @return distance to the pose */ public static double getDistanceToPose(Pose2d robotPose, Pose2d targetPose) { return robotPose.getTranslation().getDistance(targetPose.getTranslation()); 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 f125ea2ac3..4cc69bd7ba 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -80,7 +80,7 @@ public class PhotonCameraSim implements AutoCloseable { private double minTargetAreaPercent; private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest; - private AprilTagFieldLayout tagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + private final AprilTagFieldLayout tagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); // video stream simulation private final CvSource videoSimRaw; diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 35b0ad190f..7f8bc0a81d 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -79,7 +79,7 @@ public class SimCameraProperties { private double avgLatencyMs = 0; private double latencyStdDevMs = 0; // util - private List viewplanes = new ArrayList<>(); + private final List viewplanes = new ArrayList<>(); /** Default constructor which is the same as {@link #PERFECT_90DEG} */ public SimCameraProperties() { @@ -215,8 +215,8 @@ public void setCalibration( 0, getPixelPitch(resHeight).plus(new Rotation2d(-Math.PI / 2)).getRadians(), 0)) }; viewplanes.clear(); - for (int i = 0; i < p.length; i++) { - viewplanes.add(new DMatrix3(p[i].getX(), p[i].getY(), p[i].getZ())); + for (Translation3d translation3d : p) { + viewplanes.add(new DMatrix3(translation3d.getX(), translation3d.getY(), translation3d.getZ())); } } @@ -457,8 +457,7 @@ public Pair getVisibleLine( // check if the ends of the line segment are visible boolean aVisible = true; boolean bVisible = true; - for (int i = 0; i < viewplanes.size(); i++) { - var normal = viewplanes.get(i); + for (DMatrix3 normal : viewplanes) { double aVisibility = CommonOps_DDF3.dot(av, normal); if (aVisibility < 0) aVisible = false; double bVisibility = CommonOps_DDF3.dot(bv, normal); @@ -467,7 +466,7 @@ public Pair getVisibleLine( if (aVisibility <= 0 && bVisibility <= 0) return new Pair<>(null, null); } // both ends are inside frustum - if (aVisible && bVisible) return new Pair<>(Double.valueOf(0), Double.valueOf(1)); + if (aVisible && bVisible) return new Pair<>((double) 0, 1.0); // parametrized (t=0 at a, t=1 at b) intersections with viewplanes double[] intersections = {Double.NaN, Double.NaN, Double.NaN, Double.NaN}; @@ -546,8 +545,8 @@ public Pair getVisibleLine( } // one viewplane intersection else if (!Double.isNaN(inter1)) { - if (aVisible) return new Pair<>(Double.valueOf(0), inter1); - if (bVisible) return new Pair<>(inter1, Double.valueOf(1)); + if (aVisible) return new Pair<>((double) 0, inter1); + if (bVisible) return new Pair<>(inter1, 1.0); return new Pair<>(inter1, null); } // no intersections 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..95e643bc35 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java @@ -121,7 +121,7 @@ public SimVisionSystem( public void addSimVisionTarget(SimVisionTarget target) { tgtList.add(target); dbgField - .getObject("Target " + Integer.toString(target.targetID)) + .getObject("Target " + target.targetID) .setPose(target.targetPose.toPose2d()); } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index 1bbe921006..7e4d7eddf2 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -374,8 +374,8 @@ public static void drawTagDetection(int id, Point[] dstPoints, Mat destination) /** * Set the field dimensions that are used for drawing the field wireframe. * - * @param fieldLengthMeters - * @param fieldWidthMeters + * @param fieldLengthMeters field length in meters + * @param fieldWidthMeters field wideth in meters */ public static void setFieldDimensionsMeters(double fieldLengthMeters, double fieldWidthMeters) { fieldLength = fieldLengthMeters; @@ -501,8 +501,8 @@ public static List polyFrom3dLines( if (inter.getSecond() == null) continue; // cull line to the inside of the camera fulcrum - double inter1 = inter.getFirst().doubleValue(); - double inter2 = inter.getSecond().doubleValue(); + double inter1 = inter.getFirst(); + double inter2 = inter.getSecond(); var baseDelta = ptb.minus(pta); var old_pta = pta; if (inter1 > 0) pta = old_pta.plus(baseDelta.times(inter1)); @@ -510,11 +510,9 @@ public static List polyFrom3dLines( baseDelta = ptb.minus(pta); // project points into 2d - var poly = new ArrayList(); - poly.addAll( - Arrays.asList( - OpenCVHelp.projectPoints( - prop.getIntrinsics(), prop.getDistCoeffs(), camRt, List.of(pta, ptb)))); + var poly = new ArrayList<>(Arrays.asList( + OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), camRt, List.of(pta, ptb)))); var pxa = poly.get(0); var pxb = poly.get(1); @@ -526,7 +524,7 @@ public static List polyFrom3dLines( for (int j = 0; j < subdivisions; j++) { subPts.add(pta.plus(subDelta.times(j + 1))); } - if (subPts.size() > 0) { + if (!subPts.isEmpty()) { poly.addAll( 1, Arrays.asList( diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index 573ad77e91..eb4720a21a 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -176,8 +176,7 @@ public Optional getCameraPose(PhotonCameraSim cameraSim) { */ public Optional getCameraPose(PhotonCameraSim cameraSim, double timeSeconds) { var robotToCamera = getRobotToCamera(cameraSim, timeSeconds); - if (robotToCamera.isEmpty()) return Optional.empty(); - return Optional.of(getRobotPose(timeSeconds).plus(robotToCamera.get())); + return robotToCamera.map(transform3d -> getRobotPose(timeSeconds).plus(transform3d)); } /** @@ -405,6 +404,6 @@ public void update(Pose3d robotPoseMeters) { } } if (processed) dbgField.getObject("visibleTargetPoses").setPoses(visTgtPoses2d); - if (cameraPoses2d.size() != 0) dbgField.getObject("cameras").setPoses(cameraPoses2d); + if (!cameraPoses2d.isEmpty()) dbgField.getObject("cameras").setPoses(cameraPoses2d); } } diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 1b386cc747..4eaffa0fea 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -69,7 +69,7 @@ public static void init() { e.printStackTrace(); } - List tagList = new ArrayList(2); + List tagList = new ArrayList<>(2); tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d()))); tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d()))); double fieldLength = Units.feetToMeters(54.0); @@ -619,7 +619,7 @@ void averageBestPoses() { assertEquals(2.15, pose.getZ(), .01); } - private class PhotonCameraInjector extends PhotonCamera { + private static class PhotonCameraInjector extends PhotonCamera { public PhotonCameraInjector() { super("Test"); } diff --git a/photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java b/photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java index 6dc5a5854b..3009de49e9 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java @@ -30,7 +30,7 @@ import org.junit.jupiter.api.Test; public class PhotonVersionTest { - public static final boolean versionMatches(String versionString, String other) { + public static boolean versionMatches(String versionString, String other) { String c = versionString; Pattern p = Pattern.compile("v[0-9]+.[0-9]+.[0-9]+"); Matcher m = p.matcher(c); 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 196d6c845b..ed53a17b80 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -49,8 +49,8 @@ import org.photonvision.targeting.TargetCorner; public final class OpenCVHelp { - private static Rotation3d NWU_TO_EDN; - private static Rotation3d EDN_TO_NWU; + private static final Rotation3d NWU_TO_EDN; + private static final Rotation3d EDN_TO_NWU; // Creating a cscore object is sufficient to load opencv, per // https://www.chiefdelphi.com/t/unsatisfied-link-error-when-simulating-java-robot-code-using-opencv/426731/4 @@ -199,7 +199,7 @@ public static List pointsToCorners(MatOfPoint2f matInput) { *

({1,2,3}, true, 1) == {3,2,1} * * @param Element type - * @param elements + * @param elements list elements * @param backwards If indexing should happen in reverse (0, size-1, size-2, ...) * @param shiftStart How much the inital index should be shifted (instead of starting at index 0, * start at shiftStart, negated if backwards) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java index 4792c3fb72..a3ddaaa6f0 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java @@ -41,7 +41,7 @@ public static MultiTargetPNPResult createFromPacket(Packet packet) { var results = PNPResult.createFromPacket(packet); var ids = new ArrayList(MAX_IDS); for (int i = 0; i < MAX_IDS; i++) { - int targetId = (int) packet.decodeShort(); + int targetId = packet.decodeShort(); if (targetId > -1) ids.add(targetId); } return new MultiTargetPNPResult(results, ids); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java index edb4c0fc49..ab7aa5f46f 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java @@ -102,14 +102,13 @@ public static PNPResult createFromPacket(Packet packet) { } } - public Packet populatePacket(Packet packet) { + public void 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 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 b0910ea253..9a8ce65db9 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -130,7 +130,7 @@ public void setTimestampSeconds(double timestampSeconds) { * @return Whether the pipeline has targets. */ public boolean hasTargets() { - return targets.size() > 0; + return !targets.isEmpty(); } /** @@ -143,7 +143,7 @@ public List getTargets() { } /** - * Return the latest mulit-target result. Be sure to check + * Return the latest multi-target result. Be sure to check * getMultiTagResult().estimatedPose.isPresent before using the pose estimate! */ public MultiTargetPNPResult getMultiTagResult() { @@ -197,7 +197,7 @@ public Packet populatePacket(Packet packet) { public int hashCode() { final int prime = 31; int result = 1; - result = prime * result + ((targets == null) ? 0 : targets.hashCode()); + result = prime * result + targets.hashCode(); long temp; temp = Double.doubleToLongBits(latencyMillis); result = prime * result + (int) (temp ^ (temp >>> 32)); @@ -213,9 +213,7 @@ public boolean equals(Object obj) { 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 (!targets.equals(other.targets)) return false; if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) return false; if (Double.doubleToLongBits(timestampSeconds) 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..dc847ec5fc 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -198,9 +198,9 @@ public boolean equals(Object obj) { 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); + for (TargetCorner targetCorner : list) { + packet.encode(targetCorner.x); + packet.encode(targetCorner.y); } } From 45c4501ba8f20f6e7b0f873ac7bdbcb53b8f2570 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 14 Nov 2023 18:41:10 -0500 Subject: [PATCH 6/9] Formatting fixes --- .../java/org/photonvision/simulation/PhotonCameraSim.java | 3 ++- .../org/photonvision/simulation/SimCameraProperties.java | 3 ++- .../java/org/photonvision/simulation/SimVisionSystem.java | 4 +--- .../java/org/photonvision/simulation/VideoSimUtil.java | 8 +++++--- 4 files changed, 10 insertions(+), 8 deletions(-) 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 4cc69bd7ba..b602b0194d 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -80,7 +80,8 @@ public class PhotonCameraSim implements AutoCloseable { private double minTargetAreaPercent; private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest; - private final AprilTagFieldLayout tagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + private final AprilTagFieldLayout tagLayout = + AprilTagFields.kDefaultField.loadAprilTagLayoutField(); // video stream simulation private final CvSource videoSimRaw; diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 7f8bc0a81d..6d157fb252 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -216,7 +216,8 @@ public void setCalibration( }; viewplanes.clear(); for (Translation3d translation3d : p) { - viewplanes.add(new DMatrix3(translation3d.getX(), translation3d.getY(), translation3d.getZ())); + viewplanes.add( + new DMatrix3(translation3d.getX(), translation3d.getY(), translation3d.getZ())); } } 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 95e643bc35..df16412de5 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java @@ -120,9 +120,7 @@ public SimVisionSystem( */ public void addSimVisionTarget(SimVisionTarget target) { tgtList.add(target); - dbgField - .getObject("Target " + target.targetID) - .setPose(target.targetPose.toPose2d()); + dbgField.getObject("Target " + target.targetID).setPose(target.targetPose.toPose2d()); } /** diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index 7e4d7eddf2..b5f5a37efd 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -510,9 +510,11 @@ public static List polyFrom3dLines( baseDelta = ptb.minus(pta); // project points into 2d - var poly = new ArrayList<>(Arrays.asList( - OpenCVHelp.projectPoints( - prop.getIntrinsics(), prop.getDistCoeffs(), camRt, List.of(pta, ptb)))); + var poly = + new ArrayList<>( + Arrays.asList( + OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), camRt, List.of(pta, ptb)))); var pxa = poly.get(0); var pxb = poly.get(1); From 5769e93823dd9ac9e657274108c12660c63f608a Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 15 Nov 2023 18:07:18 -0500 Subject: [PATCH 7/9] add multisubscriber back --- photon-lib/src/main/java/org/photonvision/PhotonCamera.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 3f201cfe33..e40df06e6d 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -146,6 +146,10 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { ledModeRequest = photonvision_root_table.getIntegerTopic("ledModeRequest").publish(); ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1); versionEntry = photonvision_root_table.getStringTopic("version").subscribe(""); + + // Existing is enough to make this multisubscriber do its thing + MultiSubscriber m_topicNameSubscriber = new MultiSubscriber( + instance, new String[]{"/photonvision/"}, PubSubOption.topicsOnly(true)); } /** From 6672f46e5f8ca987e92b82f29a61e7f40b5d9513 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 15 Nov 2023 18:08:16 -0500 Subject: [PATCH 8/9] Formatting fixes --- photon-lib/src/main/java/org/photonvision/PhotonCamera.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index e40df06e6d..8b46a2c372 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -36,6 +36,7 @@ import edu.wpi.first.networktables.IntegerEntry; import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.IntegerSubscriber; +import edu.wpi.first.networktables.MultiSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.PubSubOption; @@ -148,8 +149,9 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { versionEntry = photonvision_root_table.getStringTopic("version").subscribe(""); // Existing is enough to make this multisubscriber do its thing - MultiSubscriber m_topicNameSubscriber = new MultiSubscriber( - instance, new String[]{"/photonvision/"}, PubSubOption.topicsOnly(true)); + MultiSubscriber m_topicNameSubscriber = + new MultiSubscriber( + instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true)); } /** From 99bddea876cebfb7a35eb8cf175edb2bed7a2584 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 15 Nov 2023 18:19:47 -0500 Subject: [PATCH 9/9] make comments better about x and y relationship --- .../main/java/org/photonvision/simulation/VideoSimUtil.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index b5f5a37efd..8b5c787b78 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -374,8 +374,8 @@ public static void drawTagDetection(int id, Point[] dstPoints, Mat destination) /** * Set the field dimensions that are used for drawing the field wireframe. * - * @param fieldLengthMeters field length in meters - * @param fieldWidthMeters field wideth in meters + * @param fieldLengthMeters field length in meters (x direction) + * @param fieldWidthMeters field width in meters (y direction) */ public static void setFieldDimensionsMeters(double fieldLengthMeters, double fieldWidthMeters) { fieldLength = fieldLengthMeters;