[photonlib] Simulation robustness (#874)

- `PNPResults` can now be empty (`isPresent` = false)
- solvePNP methods actually handle errors and return empty `PNPResults`
  - This reveals an odd error where some inputs to `solvePNP_SQUARE()` resulted in an estimated transform with NaN values, and attempts to handle it
- Overwrites java changes from #817 since #742 had duplicate fixes
- Minor bugfixes

photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java

@@ -31,7 +31,6 @@ import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.*;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Objects;
@@ -40,7 +39,7 @@ import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
 
 public class VisionEstimation {
-    /** Get the visible {@link AprilTag}s according the tag layout using the visible tag IDs. */
+    /** Get the visible {@link AprilTag}s which are in the tag layout using the visible tag IDs. */
     public static List<AprilTag> getVisibleLayoutTags(
             List<PhotonTrackedTarget> visTags, AprilTagFieldLayout tagLayout) {
         return visTags.stream()
@@ -55,19 +54,23 @@ public class VisionEstimation {
     }
 
     /**
-     * Performs solvePNP using 3d-2d point correspondences to estimate the field-to-camera
-     * transformation. If only one tag is visible, the result may have an alternate solution.
+     * Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the
+     * field-to-camera transformation. If only one tag is visible, the result may have an alternate
+     * solution.
      *
      * <p><b>Note:</b> The returned transformation is from the field origin to the camera pose!
-     * (Unless you only feed this one tag??)
+     *
+     * <p>With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}
+     *
+     * <p>With multiple tags: {@link OpenCVHelp#solvePNP_SQPNP}
      *
      * @param cameraMatrix The camera intrinsics matrix in standard opencv form
      * @param distCoeffs The camera distortion matrix in standard opencv form
      * @param visTags The visible tags reported by PV
      * @param tagLayout The known tag layout on the field
-     * @return The transformation that maps the field origin to the camera pose
+     * @return The transformation that maps the field origin to the camera pose. Ensure the {@link
+     *     PNPResults} are present before utilizing them.
      */
-    @Deprecated
     public static PNPResults estimateCamPosePNP(
             Matrix<N3, N3> cameraMatrix,
             Matrix<N5, N1> distCoeffs,
@@ -92,24 +95,12 @@ public class VisionEstimation {
             var camToTag =
                     OpenCVHelp.solvePNP_SQUARE(
                             cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners);
+            if (!camToTag.isPresent) return new PNPResults();
             var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
             var altPose = new Pose3d();
             if (camToTag.ambiguity != 0)
                 altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse());
 
-            var bestTagToCam = camToTag.best.inverse();
-            SmartDashboard.putNumberArray(
-                    "multiTagBest_internal",
-                    new double[] {
-                        bestTagToCam.getX(),
-                        bestTagToCam.getY(),
-                        bestTagToCam.getZ(),
-                        bestTagToCam.getRotation().getQuaternion().getW(),
-                        bestTagToCam.getRotation().getQuaternion().getX(),
-                        bestTagToCam.getRotation().getQuaternion().getY(),
-                        bestTagToCam.getRotation().getQuaternion().getZ()
-                    });
-
             var o = new Pose3d();
             return new PNPResults(
                     new Transform3d(o, bestPose),
@@ -123,6 +114,7 @@ public class VisionEstimation {
             var objectTrls = new ArrayList<Translation3d>();
             for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose));
             var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners);
+            if (!camToOrigin.isPresent) return new PNPResults();
             return new PNPResults(
                     camToOrigin.best.inverse(),
                     camToOrigin.alt.inverse(),
@@ -131,59 +123,4 @@ public class VisionEstimation {
                     camToOrigin.altReprojErr);
         }
     }
-
-    /**
-     * Performs solvePNP using 3d-2d point correspondences to estimate the field-to-camera
-     * transformation. If only one tag is visible, the result may have an alternate solution.
-     *
-     * <p><b>Note:</b> The returned transformation is from the field origin to the camera pose!
-     *
-     * @param cameraMatrix the camera intrinsics matrix in standard opencv form
-     * @param distCoeffs the camera distortion matrix in standard opencv form
-     * @param corners The visible tag corners in the 2d image
-     * @param knownTags The known tag field poses corresponding to the visible tag IDs
-     * @return The transformation that maps the field origin to the camera pose
-     */
-    public static PNPResults estimateCamPoseSqpnp(
-            Matrix<N3, N3> cameraMatrix,
-            Matrix<N5, N1> distCoeffs,
-            List<TargetCorner> corners,
-            List<AprilTag> knownTags) {
-        if (knownTags == null
-                || corners == null
-                || corners.size() != knownTags.size() * 4
-                || knownTags.size() == 0) {
-            return new PNPResults();
-        }
-        var objectTrls = new ArrayList<Translation3d>();
-        for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.vertices);
-        var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners);
-        // var camToOrigin = OpenCVHelp.solveTagsPNPRansac(prop, objectTrls, corners);
-        return new PNPResults(
-                camToOrigin.best.inverse(),
-                camToOrigin.alt.inverse(),
-                camToOrigin.ambiguity,
-                camToOrigin.bestReprojErr,
-                camToOrigin.altReprojErr);
-    }
-
-    /**
-     * The best estimated transformation (Rotation-translation composition) that maps a set of
-     * translations onto another with point correspondences, and its RMSE.
-     */
-    public static class SVDResults {
-        public final RotTrlTransform3d trf;
-
-        /** If the result is invalid, this value is -1 */
-        public final double rmse;
-
-        public SVDResults() {
-            this(new RotTrlTransform3d(), -1);
-        }
-
-        public SVDResults(RotTrlTransform3d trf, double rmse) {
-            this.trf = trf;
-            this.rmse = rmse;
-        }
-    }
 }