[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/simulation/PhotonCameraSim.java

@@ -70,7 +70,7 @@ public class PhotonCameraSim implements AutoCloseable {
     private long nextNTEntryTime = WPIUtilJNI.now();
 
     private double maxSightRangeMeters = Double.MAX_VALUE;
-    private static final double kDefaultMinAreaPx = 90;
+    private static final double kDefaultMinAreaPx = 100;
     private double minTargetAreaPercent;
     private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
 
@@ -356,6 +356,7 @@ public class PhotonCameraSim implements AutoCloseable {
                                 prop.getDistCoeffs(),
                                 tgt.getModel().vertices,
                                 noisyTargetCorners);
+                if (!pnpSim.isPresent) continue;
                 centerRot =
                         prop.getPixelRot(
                                 OpenCVHelp.projectPoints(
@@ -459,6 +460,8 @@ public class PhotonCameraSim implements AutoCloseable {
             ts.targetPoseEntry.set(poseData, receiveTimestamp);
         }
 
+        ts.cameraIntrinsicsPublisher.set(prop.getIntrinsics().getData(), receiveTimestamp);
+        ts.cameraDistortionPublisher.set(prop.getDistCoeffs().getData(), receiveTimestamp);
         ts.heartbeatPublisher.set(heartbeatCounter++, receiveTimestamp);
     }
 }

photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java

@@ -109,19 +109,10 @@ public class VideoSimUtil {
         name = name.substring(0, name.length() - idString.length()) + idString;
 
         var resource = VideoSimUtil.class.getResource(kResourceTagImagesPath + name + ".png");
-        // local IDE tests
-        String path = kLocalTagImagesPath + name + ".png";
-        // gradle tests
-        if (resource != null) {
-            path = resource.getPath();
 
-            // TODO why did we have this previously?
-            // if (path.startsWith("/")) path = path.substring(1);
-        }
         Mat result = new Mat();
-        if (!path.startsWith("file")) result = Imgcodecs.imread(path, Imgcodecs.IMREAD_GRAYSCALE);
         // reading jar file
-        if (result.empty()) {
+        if (resource != null && resource.getPath().startsWith("file")) {
             BufferedImage buf;
             try {
                 buf = ImageIO.read(resource);
@@ -140,6 +131,8 @@ public class VideoSimUtil {
                 }
             }
         }
+        // local IDE tests
+        else result = Imgcodecs.imread(kLocalTagImagesPath + name + ".png", Imgcodecs.IMREAD_GRAYSCALE);
         return result;
     }
 

photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java

@@ -355,7 +355,7 @@ public class VisionSystemSim {
 
             // use camera pose from the image capture timestamp
             Pose3d lateRobotPose = getRobotPose(timestampCapture);
-            Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim).get());
+            Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim, timestampCapture).get());
             cameraPose2ds.add(lateCameraPose.toPose2d());
 
             // process a PhotonPipelineResult with visible targets