[WIP] Simulation Overhaul (#742)

### What does this do?

- Deprecates previous sim classes
- Has a `CameraProperties` class for describing a camera's basic/calibration info, and performance values for simulation. Calibration values can be loaded from the `config.json` in the settings exported by photonvision.
- `OpenCVHelp` provides convenience functions for using opencv methods with wpilib/photonvision classes, mainly to project 3d points to a camera's 2d image and perform solvePnP with the above camera calibration info.
  - `TargetModel`s describe the 3d shape of a target, both for projecting into the camera's 2d image and use in solvePnP.
- `PhotonCameraSim` uses camera properties to simulate how 3d targets would appear in its view, and has simulated noise, latency, and FPS. For apriltags, the best/alternate camera-to-target transform is also estimated with solvePnP.
  - `VideoSimUtil` has helper functions for drawing apriltags to a simulated raw and processed MJPEG stream for each camera using the projected tag corners.
- `VisionSystemSim` stores `VisionTargetSim`s and `PhotonCameraSim`s, and is periodically updated with the robot's simulated pose. When updating, camera sims are automatically processed and published with their visible targets from their respective poses with proper latency.

### What's still not working?

- Mac Arm builds are broken
- More examples
- Update website/docs

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

@@ -25,20 +25,34 @@
 package org.photonvision.estimation;
 
 import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.Matrix;
 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.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.Objects;
+import java.util.stream.Collectors;
+import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
 
 public class VisionEstimation {
-    public static final TargetModel kTagModel =
-            new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6));
+    /** Get the visible {@link AprilTag}s according the tag layout using the visible tag IDs. */
+    public static List<AprilTag> getVisibleLayoutTags(
+            List<PhotonTrackedTarget> visTags, AprilTagFieldLayout tagLayout) {
+        return visTags.stream()
+                .map(
+                        t -> {
+                            int id = t.getFiducialId();
+                            var maybePose = tagLayout.getTagPose(id);
+                            return maybePose.map(pose3d -> new AprilTag(id, pose3d)).orElse(null);
+                        })
+                .filter(Objects::nonNull)
+                .collect(Collectors.toList());
+    }
 
     /**
      * Performs solvePNP using 3d-2d point correspondences to estimate the field-to-camera
@@ -47,29 +61,37 @@ public class VisionEstimation {
      * <p><b>Note:</b> The returned transformation is from the field origin to the camera pose!
      * (Unless you only feed this one tag??)
      *
-     * @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
+     * @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
      */
     @Deprecated
     public static PNPResults estimateCamPosePNP(
             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) {
+            List<PhotonTrackedTarget> visTags,
+            AprilTagFieldLayout tagLayout) {
+        if (tagLayout == null
+                || visTags == null
+                || tagLayout.getTags().size() == 0
+                || visTags.size() == 0) {
             return new PNPResults();
         }
+
+        var corners = new ArrayList<TargetCorner>();
+        for (var tag : visTags) corners.addAll(tag.getDetectedCorners());
+        var knownTags = getVisibleLayoutTags(visTags, tagLayout);
+        if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
+            return new PNPResults();
+        }
+
         // single-tag pnp
-        if (corners.size() == 4) {
+        if (visTags.size() == 1) {
             var camToTag =
                     OpenCVHelp.solvePNP_SQUARE(
-                            cameraMatrix, distCoeffs, kTagModel.getFieldVertices(knownTags.get(0).pose), corners);
+                            cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners);
             var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
             var altPose = new Pose3d();
             if (camToTag.ambiguity != 0)
@@ -99,9 +121,8 @@ public class VisionEstimation {
         // multi-tag pnp
         else {
             var objectTrls = new ArrayList<Translation3d>();
-            for (var tag : knownTags) objectTrls.addAll(kTagModel.getFieldVertices(tag.pose));
+            for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose));
             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(),
@@ -135,7 +156,7 @@ public class VisionEstimation {
             return new PNPResults();
         }
         var objectTrls = new ArrayList<Translation3d>();
-        for (var tag : knownTags) objectTrls.addAll(kTagModel.getFieldVertices(tag.pose));
+        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(