[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

.gitignore

@@ -145,6 +145,7 @@ build/spotlessJava
 build/*
 build
 photon-lib/src/main/java/org/photonvision/PhotonVersion.java
+photon-lib/bin/main/images/*
 /photonlib-java-examples/bin/
 photon-lib/src/generate/native/include/PhotonVersion.h
 .gitattributes

photon-lib/build.gradle

@@ -34,20 +34,29 @@ nativeUtils.platformConfigs.linuxathena.cppCompiler.args.add("-Wno-deprecated-an
 dependencies {
     implementation project(":photon-targeting")
 
+    // WPILib deps
     implementation wpilibTools.deps.wpilibJava("wpiutil")
+    implementation wpilibTools.deps.wpilibJava("cameraserver")
+    implementation wpilibTools.deps.wpilibJava("cscore")
     implementation wpilibTools.deps.wpilibJava("wpimath")
     implementation wpilibTools.deps.wpilibJava("wpinet")
     implementation wpilibTools.deps.wpilibJava("hal")
     implementation wpilibTools.deps.wpilibJava("ntcore")
     implementation wpilibTools.deps.wpilibJava("wpilibj")
     implementation wpilibTools.deps.wpilibJava("apriltag")
 
-    testImplementation wpilibTools.deps.wpilibJava("cscore")
+    // Jackson
+    implementation "com.fasterxml.jackson.core:jackson-annotations:2.12.4"
+    implementation "com.fasterxml.jackson.core:jackson-core:2.12.4"
+    implementation "com.fasterxml.jackson.core:jackson-databind:2.12.4"
+
     implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-java:$opencvVersion"
     implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-jni:$opencvVersion:$jniPlatform"
+
     implementation "org.ejml:ejml-simple:0.41"
 
     // Junit
+    testImplementation wpilibTools.deps.wpilibJava("cscore")
     testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
     testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
     testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2")
@@ -157,6 +166,7 @@ def testNativeTasks = wpilibTools.createExtractionTasks {
 
 testNativeTasks.addToSourceSetResources(sourceSets.test)
 
+testNativeConfig.dependencies.add wpilibTools.deps.cscore()
 testNativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
 testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
 testNativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")

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

@@ -51,10 +51,10 @@
 import org.photonvision.targeting.PhotonPipelineResult;
 
 /** Represents a camera that is connected to PhotonVision. */
-public class PhotonCamera {
-    static final String kTableName = "photonvision";
+public class PhotonCamera implements AutoCloseable {
+    public static final String kTableName = "photonvision";
 
-    protected final NetworkTable cameraTable;
+    private final NetworkTable cameraTable;
     RawSubscriber rawBytesEntry;
     BooleanPublisher driverModePublisher;
     BooleanSubscriber driverModeSubscriber;
@@ -73,6 +73,7 @@ public class PhotonCamera {
     private DoubleArraySubscriber cameraIntrinsicsSubscriber;
     private DoubleArraySubscriber cameraDistortionSubscriber;
 
+    @Override
     public void close() {
         rawBytesEntry.close();
         driverModePublisher.close();
@@ -151,9 +152,7 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
 
         m_topicNameSubscriber =
                 new MultiSubscriber(
-                        instance,
-                        new String[] {"/photonvision/"},
-                        new PubSubOption[] {PubSubOption.topicsOnly(true)});
+                        instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
     }
 
     /**
@@ -186,7 +185,7 @@ public PhotonPipelineResult getLatestResult() {
         ret.createFromPacket(packet);
 
         // Set the timestamp of the result.
-        // getLatestChange returns in microseconds so we divide by 1e6 to convert to seconds.
+        // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
         ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3);
 
         // Return result.
@@ -334,6 +333,14 @@ public Optional<Matrix<N5, N1>> getDistCoeffs() {
         } else return Optional.empty();
     }
 
+    /**
+     * Gets the NetworkTable representing this camera's subtable. You probably don't ever need to call
+     * this.
+     */
+    public final NetworkTable getCameraTable() {
+        return cameraTable;
+    }
+
     private void verifyVersion() {
         if (!VERSION_CHECK_ENABLED) return;
 

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

@@ -285,7 +285,7 @@ public static List<TargetCorner> projectPoints(
     /**
      * Undistort 2d image points using a given camera's intrinsics and distortion.
      *
-     * <p>2d image points from projectPoints(CameraProperties, Pose3d, List) projectPoints} will
+     * <p>2d image points from projectPoints(CameraProperties, Pose3d, List) projectPoints will
      * naturally be distorted, so this operation is important if the image points need to be directly
      * used (e.g. 2d yaw/pitch).
      *

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

@@ -106,14 +106,14 @@ public Translation3d apply(Translation3d trl) {
     ;
 
     public List<Translation3d> applyTrls(List<Translation3d> trls) {
-        return trls.stream().map(t -> apply(t)).collect(Collectors.toList());
+        return trls.stream().map(this::apply).collect(Collectors.toList());
     }
 
     public Pose3d apply(Pose3d pose) {
         return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl), pose.getRotation().plus(rot));
     }
 
     public List<Pose3d> applyPoses(List<Pose3d> poses) {
-        return poses.stream().map(p -> apply(p)).collect(Collectors.toList());
+        return poses.stream().map(this::apply).collect(Collectors.toList());
     }
 }

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 static PNPResults estimateCamPosePNP(
         // 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 static PNPResults estimateCamPoseSqpnp(
             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(