[photon-targeting][photon-lib] Add tests to the targeting classes and…

… cleanup photon-lib & photon-targeting (PhotonVision#1007)

* Make MultiTagPNPResult and PNPResult singular

* add java tests

* Formatting fixes

* bring in the rest of the little stuff

* final things

* Formatting fixes

* add multisubscriber back

* Formatting fixes

* make comments better about x and y relationship

photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java

@@ -24,21 +24,21 @@
 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;
 
 /** Estimate the camera pose given multiple Apriltag observations */
 public class MultiTargetPNPPipe
         extends CVPipe<
-                List<TrackedTarget>, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
+                List<TrackedTarget>, MultiTargetPNPResult, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
     private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule);
 
     private boolean hasWarned = false;
 
     @Override
-    protected MultiTargetPNPResults process(List<TrackedTarget> targetList) {
+    protected MultiTargetPNPResult process(List<TrackedTarget> targetList) {
         if (params == null
                 || params.cameraCoefficients == null
                 || params.cameraCoefficients.getCameraIntrinsicsMat() == null
@@ -48,13 +48,13 @@ protected MultiTargetPNPResults process(List<TrackedTarget> 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<TrackedTarget> targetList) {
+    private MultiTargetPNPResult calculateCameraInField(List<TrackedTarget> targetList) {
         // Find tag IDs that exist in the tag layout
         var tagIDsUsed = new ArrayList<Integer>();
         for (var target : targetList) {
@@ -64,7 +64,7 @@ private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> 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<TrackedTarget> targetL
                         params.atfl,
                         params.targetModel);
 
-        return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);
+        return new MultiTargetPNPResult(estimatedPose, tagIDsUsed);
     }
 
     public static class MultiTargetPNPPipeParams {

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;

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;

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));
     }

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<TrackedTarget> targets;
     public final Frame inputAndOutputFrame;
-    public MultiTargetPNPResults multiTagResult;
+    public MultiTargetPNPResult multiTagResult;
 
     public CVPipelineResult(
             double processingNanos, double fps, List<TrackedTarget> targets, Frame inputFrame) {
-        this(processingNanos, fps, targets, new MultiTargetPNPResults(), inputFrame);
+        this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame);
     }
 
     public CVPipelineResult(
             double processingNanos,
             double fps,
             List<TrackedTarget> 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<TrackedTarget> targets,
-            MultiTargetPNPResults multiTagResults) {
-        this(processingNanos, fps, targets, multiTagResults, null);
+            MultiTargetPNPResult multiTagResult) {
+        this(processingNanos, fps, targets, multiTagResult, null);
     }
 
     public boolean hasTargets() {

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")

photon-lib/publish.gradle

@@ -152,7 +152,7 @@ publishing {
 
 tasks.withType(PublishToMavenRepository) {
     doFirst {
-        println("Publishing to " + repository.url);
+        println("Publishing to " + repository.url)
     }
 }
 

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;
@@ -43,7 +41,6 @@
 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 +70,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 +95,6 @@ public void close() {
         pipelineIndexRequest.close();
         cameraIntrinsicsSubscriber.close();
         cameraDistortionSubscriber.close();
-        atflPublisher.close();
     }
 
     private final String path;
@@ -111,20 +106,14 @@ 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;
     }
 
     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.
      *
@@ -159,11 +148,8 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         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 =
+        // Existing is enough to make this multisubscriber do its thing
+        MultiSubscriber m_topicNameSubscriber =
                 new MultiSubscriber(
                         instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
     }
@@ -329,49 +315,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<Matrix<N3, N3>> getCameraMatrix() {
         var cameraMatrix = cameraIntrinsicsSubscriber.get();
         if (cameraMatrix != null && cameraMatrix.length == 9) {
@@ -428,7 +374,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(

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -443,15 +443,15 @@ private Optional<EstimatedRobotPose> 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(

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());