Fix driver mode pub/sub key mixup (#810)

* Fix non-checked optional in mulit-tag

* Fix fallback return

* Run format

.github/workflows/main.yml

@@ -11,6 +11,7 @@ on:
       - 'v*'
   pull_request:
     branches: [ master ]
+  merge_group:
 
 jobs:
   # This job builds the client (web view).

.gitignore

@@ -153,3 +153,5 @@ photon-server/src/main/resources/nativelibraries/apriltag/*
 
 photonlib-java-examples/*/vendordeps/*
 photonlib-cpp-examples/*/vendordeps/*
+
+*/networktables.json

build.gradle

@@ -26,7 +26,7 @@ allprojects {
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2023.2.1"
+    wpilibVersion = "2023.3.2"
     opencvVersion = "4.6.0-4"
     joglVersion = "2.4.0-rc-20200307"
     pubVersion = versionString

photon-client/src/views/CamerasView.vue

@@ -624,7 +624,7 @@ export default {
             this.uploadSnackData = {
               color: "success",
               text:
-                "Calibration imported successfully! PhotonVision will restart in the background...",
+                "Calibration imported successfully!",
             };
             this.uploadSnack = true;
           })

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -89,7 +89,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
             ts.pipelineIndexPublisher.set(setIndex);
             // TODO: Log
         }
-        logger.debug("Successfully set pipeline index to " + newIndex);
+        logger.debug("Set pipeline index to " + newIndex);
     }
 
     private void onDriverModeChange(NetworkTableEvent entryNotification) {
@@ -102,7 +102,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
         }
 
         driverModeConsumer.accept(newDriverMode);
-        logger.debug("Successfully set driver mode to " + newDriverMode);
+        logger.debug("Set driver mode to " + newDriverMode);
     }
 
     private void removeEntries() {
@@ -119,7 +119,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
 
         pipelineIndexListener =
                 new NTDataChangeListener(
-                        ts.subTable.getInstance(), ts.pipelineIndexSubscriber, this::onPipelineIndexChange);
+                        ts.subTable.getInstance(), ts.pipelineIndexRequestSub, this::onPipelineIndexChange);
 
         driverModeListener =
                 new NTDataChangeListener(
@@ -180,6 +180,15 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
             ts.bestTargetPosY.set(0);
         }
 
+        var fsp = result.inputAndOutputFrame.frameStaticProperties;
+        if (fsp.cameraCalibration != null) {
+            ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr());
+            ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getExtrinsicsArr());
+        } else {
+            ts.cameraIntrinsicsPublisher.accept(new double[] {});
+            ts.cameraDistortionPublisher.accept(new double[] {});
+        }
+
         ts.heartbeatPublisher.set(heartbeatCounter++);
 
         // TODO...nt4... is this needed?

photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java

@@ -17,7 +17,8 @@
 
 package org.photonvision.common.hardware;
 
-import edu.wpi.first.networktables.IntegerEntry;
+import edu.wpi.first.networktables.IntegerPublisher;
+import edu.wpi.first.networktables.IntegerSubscriber;
 import java.io.IOException;
 import org.photonvision.common.ProgramStatus;
 import org.photonvision.common.configuration.ConfigManager;
@@ -47,7 +48,9 @@ public class HardwareManager {
     private final StatusLED statusLED;
 
     @SuppressWarnings("FieldCanBeLocal")
-    private final IntegerEntry ledModeEntry;
+    private IntegerSubscriber ledModeRequest;
+
+    private IntegerPublisher ledModeState;
 
     @SuppressWarnings({"FieldCanBeLocal", "unused"})
     private final NTDataChangeListener ledModeListener;
@@ -71,6 +74,15 @@ public class HardwareManager {
         this.metricsManager = new MetricsManager();
         this.metricsManager.setConfig(hardwareConfig);
 
+        ledModeRequest =
+                NetworkTablesManager.getInstance()
+                        .kRootTable
+                        .getIntegerTopic("ledModeRequest")
+                        .subscribe(-1);
+        ledModeState =
+                NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledModeState").publish();
+        ledModeState.set(VisionLEDMode.kDefault.value);
+
         CustomGPIO.setConfig(hardwareConfig);
 
         if (Platform.isRaspberryPi()) {
@@ -92,17 +104,15 @@ public class HardwareManager {
                                 hardwareConfig.ledPins,
                                 hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(0) : 0,
                                 hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(1) : 100,
-                                pigpioSocket);
+                                pigpioSocket,
+                                ledModeState::set);
 
-        ledModeEntry =
-                NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledMode").getEntry(0);
-        ledModeEntry.set(VisionLEDMode.kDefault.value);
         ledModeListener =
                 visionLED == null
                         ? null
                         : new NTDataChangeListener(
                                 NetworkTablesManager.getInstance().kRootTable.getInstance(),
-                                ledModeEntry,
+                                ledModeRequest,
                                 visionLED::onLedModeChange);
 
         Runtime.getRuntime().addShutdownHook(new Thread(this::onJvmExit));

photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java

@@ -21,6 +21,7 @@ import edu.wpi.first.networktables.NetworkTableEvent;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
 import org.photonvision.common.hardware.GPIO.CustomGPIO;
 import org.photonvision.common.hardware.GPIO.GPIOBase;
 import org.photonvision.common.hardware.GPIO.pi.PigpioException;
@@ -45,11 +46,18 @@ public class VisionLED {
 
     private int mappedBrightnessPercentage;
 
+    private Consumer<Integer> modeConsumer;
+
     public VisionLED(
-            List<Integer> ledPins, int brightnessMin, int brightnessMax, PigpioSocket pigpioSocket) {
+            List<Integer> ledPins,
+            int brightnessMin,
+            int brightnessMax,
+            PigpioSocket pigpioSocket,
+            Consumer<Integer> visionLEDmode) {
         this.brightnessMin = brightnessMin;
         this.brightnessMax = brightnessMax;
         this.pigpioSocket = pigpioSocket;
+        this.modeConsumer = visionLEDmode;
         this.ledPins = ledPins.stream().mapToInt(i -> i).toArray();
         ledPins.forEach(
                 pin -> {
@@ -123,7 +131,8 @@ public class VisionLED {
     }
 
     void onLedModeChange(NetworkTableEvent entryNotification) {
-        var newLedModeRaw = (int) entryNotification.valueData.value.getDouble();
+        var newLedModeRaw = (int) entryNotification.valueData.value.getInteger();
+        logger.debug("Got LED mode " + newLedModeRaw);
         if (newLedModeRaw != currentLedMode.value) {
             VisionLEDMode newLedMode;
             switch (newLedModeRaw) {
@@ -145,6 +154,8 @@ public class VisionLED {
                     break;
             }
             setInternal(newLedMode, true);
+
+            if (modeConsumer != null) modeConsumer.accept(newLedMode.value);
         }
     }
 

photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java

@@ -44,6 +44,10 @@ public class CameraCalibrationCoefficients implements Releasable {
     @JsonProperty("standardDeviation")
     public final double standardDeviation;
 
+    @JsonIgnore private final double[] intrinsicsArr = new double[9];
+
+    @JsonIgnore private final double[] extrinsicsArr = new double[5];
+
     @JsonCreator
     public CameraCalibrationCoefficients(
             @JsonProperty("resolution") Size resolution,
@@ -56,6 +60,10 @@ public class CameraCalibrationCoefficients implements Releasable {
         this.distCoeffs = distCoeffs;
         this.perViewErrors = perViewErrors;
         this.standardDeviation = standardDeviation;
+
+        // do this once so gets are quick
+        getCameraIntrinsicsMat().get(0, 0, intrinsicsArr);
+        getDistCoeffsMat().get(0, 0, extrinsicsArr);
     }
 
     @JsonIgnore
@@ -68,6 +76,16 @@ public class CameraCalibrationCoefficients implements Releasable {
         return distCoeffs.getAsMatOfDouble();
     }
 
+    @JsonIgnore
+    public double[] getIntrinsicsArr() {
+        return intrinsicsArr;
+    }
+
+    @JsonIgnore
+    public double[] getExtrinsicsArr() {
+        return extrinsicsArr;
+    }
+
     @JsonIgnore
     public double[] getPerViewErrors() {
         return perViewErrors;

photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java

@@ -23,6 +23,7 @@ import java.util.Arrays;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfDouble;
+import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.vision.opencv.Releasable;
 
 public class JsonMat implements Releasable {
@@ -106,4 +107,9 @@ public class JsonMat implements Releasable {
     public void release() {
         getAsMat().release();
     }
+
+    public Packet populatePacket(Packet packet) {
+        packet.encode(this.data);
+        return packet;
+    }
 }

photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java

@@ -61,10 +61,12 @@ public class FrameStaticProperties {
 
         imageArea = this.imageWidth * this.imageHeight;
 
+        // Todo -- if we have calibration, use it's center point?
         centerX = ((double) this.imageWidth / 2) - 0.5;
         centerY = ((double) this.imageHeight / 2) - 0.5;
         centerPoint = new Point(centerX, centerY);
 
+        // TODO if we have calibration use it here instead
         // pinhole model calculations
         DoubleCouple horizVertViews =
                 calculateHorizontalVerticalFoV(this.fov, this.imageWidth, this.imageHeight);

photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -363,7 +363,7 @@ public class VisionModule {
 
         if (ret != null) {
             logger.debug("Saving calibration...");
-            visionSource.getSettables().getConfiguration().addCalibration(ret);
+            visionSource.getSettables().addCalibration(ret);
         } else {
             logger.error("Calibration failed...");
         }
@@ -371,15 +371,15 @@ public class VisionModule {
         return ret;
     }
 
-    void setPipeline(int index) {
+    boolean setPipeline(int index) {
         logger.info("Setting pipeline to " + index);
         logger.info("Pipeline name: " + pipelineManager.getPipelineNickname(index));
         pipelineManager.setIndex(index);
         var pipelineSettings = pipelineManager.getPipelineSettings(index);
 
         if (pipelineSettings == null) {
-            logger.error("Config for index " + index + " was null!");
-            return;
+            logger.error("Config for index " + index + " was null! Not changing pipelines");
+            return false;
         }
 
         visionSource.getSettables().setVideoModeInternal(pipelineSettings.cameraVideoModeIndex);
@@ -422,6 +422,8 @@ public class VisionModule {
 
         visionSource.getSettables().getConfiguration().currentPipelineIndex =
                 pipelineManager.getCurrentPipelineIndex();
+
+        return true;
     }
 
     private boolean camShouldControlLEDs() {

photon-lib/build.gradle

@@ -22,6 +22,14 @@ targetCompatibility = JavaVersion.VERSION_11
 wpilibTools.deps.wpilibVersion = wpilibVersion
 println("Buidling for wpilib ${wpilibTools.deps.wpilibVersion}")
 
+// From wpilib shared/config.gradle:
+// NativeUtils adds the following OpenCV warning suppression for Linux, but not
+// for macOS
+// https://github.com/opencv/opencv/issues/20269
+nativeUtils.platformConfigs.osxuniversal.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion")
+nativeUtils.platformConfigs.linuxathena.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion")
+// nativeUtils.platformConfigs.linuxx64.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion")
+
 // Apply Java configuration
 dependencies {
     implementation project(":photon-targeting")
@@ -34,6 +42,11 @@ dependencies {
     implementation wpilibTools.deps.wpilibJava("wpilibj")
     implementation wpilibTools.deps.wpilibJava("apriltag")
 
+    testImplementation wpilibTools.deps.wpilibJava("cscore")
+    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("org.junit.jupiter:junit-jupiter-api:5.8.2")
     testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
@@ -66,6 +79,7 @@ model {
             }
             nativeUtils.useRequiredLibrary(it, "wpilib_shared")
             nativeUtils.useRequiredLibrary(it, "apriltag_shared")
+            nativeUtils.useRequiredLibrary(it, "opencv_shared")
         }
     }
     testSuites {
@@ -82,6 +96,7 @@ model {
             nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
             nativeUtils.useRequiredLibrary(it, "apriltag_shared")
             nativeUtils.useRequiredLibrary(it, "googletest_static")
+            nativeUtils.useRequiredLibrary(it, "opencv_shared")
         }
     }
 }
@@ -147,3 +162,4 @@ testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
 testNativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
 testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
 testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
+testNativeConfig.dependencies.add wpilibTools.deps.cscore()

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

@@ -25,6 +25,8 @@
 package org.photonvision;
 
 import edu.wpi.first.math.geometry.Pose3d;
+import java.util.List;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
 /** An estimated pose based on pipeline result */
 public class EstimatedRobotPose {
@@ -34,14 +36,19 @@ public class EstimatedRobotPose {
     /** The estimated time the frame used to derive the robot pose was taken */
     public final double timestampSeconds;
 
+    /** A list of the targets used to compute this pose */
+    public final List<PhotonTrackedTarget> targetsUsed;
+
     /**
      * Constructs an EstimatedRobotPose
      *
      * @param estimatedPose estimated pose
      * @param timestampSeconds timestamp of the estimate
      */
-    public EstimatedRobotPose(Pose3d estimatedPose, double timestampSeconds) {
+    public EstimatedRobotPose(
+            Pose3d estimatedPose, double timestampSeconds, List<PhotonTrackedTarget> targetsUsed) {
         this.estimatedPose = estimatedPose;
         this.timestampSeconds = timestampSeconds;
+        this.targetsUsed = targetsUsed;
     }
 }

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

@@ -24,11 +24,17 @@
 
 package org.photonvision;
 
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.*;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleArrayPublisher;
+import edu.wpi.first.networktables.DoubleArraySubscriber;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.IntegerEntry;
+import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.IntegerSubscriber;
 import edu.wpi.first.networktables.MultiSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
@@ -38,6 +44,7 @@ import edu.wpi.first.networktables.RawSubscriber;
 import edu.wpi.first.networktables.StringSubscriber;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import java.util.Optional;
 import java.util.Set;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.hardware.VisionLEDMode;
@@ -47,7 +54,7 @@ import org.photonvision.targeting.PhotonPipelineResult;
 public class PhotonCamera {
     static final String kTableName = "photonvision";
 
-    protected final NetworkTable rootTable;
+    protected final NetworkTable cameraTable;
     RawSubscriber rawBytesEntry;
     BooleanPublisher driverModePublisher;
     BooleanSubscriber driverModeSubscriber;
@@ -60,8 +67,11 @@ public class PhotonCamera {
     DoublePublisher targetSkewEntry;
     StringSubscriber versionEntry;
     IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
-    IntegerEntry pipelineIndexEntry, ledModeEntry;
+    IntegerPublisher pipelineIndexRequest, ledModeRequest;
+    IntegerSubscriber pipelineIndexState, ledModeState;
     IntegerSubscriber heartbeatEntry;
+    private DoubleArraySubscriber cameraIntrinsicsSubscriber;
+    private DoubleArraySubscriber cameraDistortionSubscriber;
 
     public void close() {
         rawBytesEntry.close();
@@ -77,8 +87,13 @@ public class PhotonCamera {
         versionEntry.close();
         inputSaveImgEntry.close();
         outputSaveImgEntry.close();
-        pipelineIndexEntry.close();
-        ledModeEntry.close();
+        pipelineIndexRequest.close();
+        pipelineIndexState.close();
+        ledModeRequest.close();
+        ledModeState.close();
+        pipelineIndexRequest.close();
+        cameraIntrinsicsSubscriber.close();
+        cameraDistortionSubscriber.close();
     }
 
     private final String path;
@@ -110,22 +125,29 @@ public class PhotonCamera {
      */
     public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         name = cameraName;
-        var mainTable = instance.getTable(kTableName);
-        this.rootTable = mainTable.getSubTable(cameraName);
-        path = rootTable.getPath();
+        var photonvision_root_table = instance.getTable(kTableName);
+        this.cameraTable = photonvision_root_table.getSubTable(cameraName);
+        path = cameraTable.getPath();
         rawBytesEntry =
-                rootTable
+                cameraTable
                         .getRawTopic("rawBytes")
                         .subscribe(
                                 "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
-        driverModePublisher = rootTable.getBooleanTopic("driverMode").publish();
-        driverModeSubscriber = rootTable.getBooleanTopic("driverModeRequest").subscribe(false);
-        inputSaveImgEntry = rootTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
-        outputSaveImgEntry = rootTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
-        pipelineIndexEntry = rootTable.getIntegerTopic("pipelineIndex").getEntry(0);
-        heartbeatEntry = rootTable.getIntegerTopic("heartbeat").subscribe(-1);
-        ledModeEntry = mainTable.getIntegerTopic("ledMode").getEntry(-1);
-        versionEntry = mainTable.getStringTopic("version").subscribe("");
+        driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
+        driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
+        inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
+        outputSaveImgEntry = cameraTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
+        pipelineIndexRequest = cameraTable.getIntegerTopic("pipelineIndexRequest").publish();
+        pipelineIndexState = cameraTable.getIntegerTopic("pipelineIndexState").subscribe(0);
+        heartbeatEntry = cameraTable.getIntegerTopic("heartbeat").subscribe(-1);
+        cameraIntrinsicsSubscriber =
+                cameraTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null);
+        cameraDistortionSubscriber =
+                cameraTable.getDoubleArrayTopic("cameraDistortion").subscribe(null);
+
+        ledModeRequest = photonvision_root_table.getIntegerTopic("ledModeRequest").publish();
+        ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1);
+        versionEntry = photonvision_root_table.getStringTopic("version").subscribe("");
 
         m_topicNameSubscriber =
                 new MultiSubscriber(
@@ -215,7 +237,7 @@ public class PhotonCamera {
      * @return The active pipeline index.
      */
     public int getPipelineIndex() {
-        return (int) pipelineIndexEntry.get(0);
+        return (int) pipelineIndexState.get(0);
     }
 
     /**
@@ -224,7 +246,7 @@ public class PhotonCamera {
      * @param index The active pipeline index.
      */
     public void setPipelineIndex(int index) {
-        pipelineIndexEntry.set(index);
+        pipelineIndexRequest.set(index);
     }
 
     /**
@@ -233,7 +255,7 @@ public class PhotonCamera {
      * @return The current LED mode.
      */
     public VisionLEDMode getLEDMode() {
-        int value = (int) ledModeEntry.get(-1);
+        int value = (int) ledModeState.get(-1);
         switch (value) {
             case 0:
                 return VisionLEDMode.kOff;
@@ -253,7 +275,7 @@ public class PhotonCamera {
      * @param led The mode to set to.
      */
     public void setLED(VisionLEDMode led) {
-        ledModeEntry.set(led.value);
+        ledModeRequest.set(led.value);
     }
 
     /**
@@ -298,6 +320,20 @@ public class PhotonCamera {
         return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
     }
 
+    public Optional<Matrix<N3, N3>> getCameraMatrix() {
+        var cameraMatrix = cameraIntrinsicsSubscriber.get();
+        if (cameraMatrix != null && cameraMatrix.length == 9) {
+            return Optional.of(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(cameraMatrix));
+        } else return Optional.empty();
+    }
+
+    public Optional<Matrix<N5, N1>> getDistCoeffs() {
+        var distCoeffs = cameraDistortionSubscriber.get();
+        if (distCoeffs != null && distCoeffs.length == 5) {
+            return Optional.of(new MatBuilder<>(Nat.N5(), Nat.N1()).fill(distCoeffs));
+        } else return Optional.empty();
+    }
+
     private void verifyVersion() {
         if (!VERSION_CHECK_ENABLED) return;
 
@@ -307,7 +343,7 @@ public class PhotonCamera {
         // Heartbeat entry is assumed to always be present. If it's not present, we
         // assume that a camera with that name was never connected in the first place.
         if (!heartbeatEntry.exists()) {
-            Set<String> cameraNames = rootTable.getInstance().getTable(kTableName).getSubTables();
+            Set<String> cameraNames = cameraTable.getInstance().getTable(kTableName).getSubTables();
             if (cameraNames.isEmpty()) {
                 DriverStation.reportError(
                         "Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!",

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

@@ -24,6 +24,7 @@
 
 package org.photonvision;
 
+import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -37,8 +38,10 @@ import java.util.HashSet;
 import java.util.List;
 import java.util.Optional;
 import java.util.Set;
+import org.photonvision.estimation.VisionEstimation;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
 
 /**
  * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
@@ -60,17 +63,22 @@ public class PhotonPoseEstimator {
         /** Choose the Pose which is closest to the last pose calculated */
         CLOSEST_TO_LAST_POSE,
 
-        /** Choose the Pose with the lowest ambiguity. */
-        AVERAGE_BEST_TARGETS
+        /** Return the average of the best target poses using ambiguity as weight. */
+        AVERAGE_BEST_TARGETS,
+
+        /** Use all visible tags to compute a single pose estimate.. */
+        MULTI_TAG_PNP
     }
 
     private AprilTagFieldLayout fieldTags;
-    private PoseStrategy strategy;
+    private PoseStrategy primaryStrategy;
+    private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
     private final PhotonCamera camera;
-    private final Transform3d robotToCamera;
+    private Transform3d robotToCamera;
 
     private Pose3d lastPose;
     private Pose3d referencePose;
+    protected double poseCacheTimestampSeconds = -1;
     private final Set<Integer> reportedErrors = new HashSet<>();
 
     /**
@@ -81,9 +89,9 @@ public class PhotonPoseEstimator {
      *     href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
      *     Coordinate System</a>.
      * @param strategy The strategy it should use to determine the best pose.
-     * @param camera PhotonCameras and
-     * @param robotToCamera Transform3d from the center of the robot to the camera mount positions
-     *     (ie, robot ➔ camera) in the <a
+     * @param camera PhotonCamera
+     * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
+     *     robot ➔ camera) in the <a
      *     href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
      *     Coordinate System</a>.
      */
@@ -93,11 +101,22 @@ public class PhotonPoseEstimator {
             PhotonCamera camera,
             Transform3d robotToCamera) {
         this.fieldTags = fieldTags;
-        this.strategy = strategy;
+        this.primaryStrategy = strategy;
         this.camera = camera;
         this.robotToCamera = robotToCamera;
     }
 
+    /** Invalidates the pose cache. */
+    private void invalidatePoseCache() {
+        poseCacheTimestampSeconds = -1;
+    }
+
+    private void checkUpdate(Object oldObj, Object newObj) {
+        if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) {
+            invalidatePoseCache();
+        }
+    }
+
     /**
      * Get the AprilTagFieldLayout being used by the PositionEstimator.
      *
@@ -113,6 +132,7 @@ public class PhotonPoseEstimator {
      * @param fieldTags the AprilTagFieldLayout
      */
     public void setFieldTags(AprilTagFieldLayout fieldTags) {
+        checkUpdate(this.fieldTags, fieldTags);
         this.fieldTags = fieldTags;
     }
 
@@ -121,8 +141,8 @@ public class PhotonPoseEstimator {
      *
      * @return the strategy
      */
-    public PoseStrategy getStrategy() {
-        return strategy;
+    public PoseStrategy getPrimaryStrategy() {
+        return primaryStrategy;
     }
 
     /**
@@ -130,8 +150,25 @@ public class PhotonPoseEstimator {
      *
      * @param strategy the strategy to set
      */
-    public void setStrategy(PoseStrategy strategy) {
-        this.strategy = strategy;
+    public void setPrimaryStrategy(PoseStrategy strategy) {
+        checkUpdate(this.primaryStrategy, strategy);
+        this.primaryStrategy = strategy;
+    }
+
+    /**
+     * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must
+     * NOT be MULTI_TAG_PNP
+     *
+     * @param strategy the strategy to set
+     */
+    public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
+        checkUpdate(this.multiTagFallbackStrategy, strategy);
+        if (strategy == PoseStrategy.MULTI_TAG_PNP) {
+            DriverStation.reportWarning(
+                    "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", null);
+            strategy = PoseStrategy.LOWEST_AMBIGUITY;
+        }
+        this.multiTagFallbackStrategy = strategy;
     }
 
     /**
@@ -150,6 +187,7 @@ public class PhotonPoseEstimator {
      * @param referencePose the referencePose to set
      */
     public void setReferencePose(Pose3d referencePose) {
+        checkUpdate(this.referencePose, referencePose);
         this.referencePose = referencePose;
     }
 
@@ -160,7 +198,7 @@ public class PhotonPoseEstimator {
      * @param referencePose the referencePose to set
      */
     public void setReferencePose(Pose2d referencePose) {
-        this.referencePose = new Pose3d(referencePose);
+        setReferencePose(new Pose3d(referencePose));
     }
 
     /**
@@ -183,12 +221,27 @@ public class PhotonPoseEstimator {
         setLastPose(new Pose3d(lastPose));
     }
 
+    /** @return The current transform from the center of the robot to the camera mount position */
+    public Transform3d getRobotToCameraTransform() {
+        return robotToCamera;
+    }
+
+    /**
+     * Useful for pan and tilt mechanisms and such.
+     *
+     * @param robotToCamera The current transform from the center of the robot to the camera mount
+     *     position
+     */
+    public void setRobotToCameraTransform(Transform3d robotToCamera) {
+        this.robotToCamera = robotToCamera;
+    }
+
     /**
      * Poll data from the configured cameras and update the estimated position of the robot. Returns
      * empty if there are no cameras set or no targets were found from the cameras.
      *
-     * @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and
-     *     pipeline results used to create the estimate
+     * @return an EstimatedRobotPose with an estimated pose, the timestamp, and targets used to create
+     *     the estimate
      */
     public Optional<EstimatedRobotPose> update() {
         if (camera == null) {
@@ -197,26 +250,65 @@ public class PhotonPoseEstimator {
         }
 
         PhotonPipelineResult cameraResult = camera.getLatestResult();
+
+        return update(cameraResult);
+    }
+
+    /**
+     * Updates the estimated position of the robot. Returns empty if there are no cameras set or no
+     * targets were found from the cameras.
+     *
+     * @param cameraResult The latest pipeline result from the camera
+     * @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and
+     *     pipeline results used to create the estimate
+     */
+    public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
+        // Time in the past -- give up, since the following if expects times > 0
+        if (cameraResult.getTimestampSeconds() < 0) {
+            return Optional.empty();
+        }
+
+        // If the pose cache timestamp was set, and the result is from the same timestamp, return an
+        // empty result
+        if (poseCacheTimestampSeconds > 0
+                && Math.abs(poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()) < 1e-6) {
+            return Optional.empty();
+        }
+
+        // Remember the timestamp of the current result used
+        poseCacheTimestampSeconds = cameraResult.getTimestampSeconds();
+
+        // If no targets seen, trivial case -- return empty result
         if (!cameraResult.hasTargets()) {
             return Optional.empty();
         }
 
+        return update(cameraResult, this.primaryStrategy);
+    }
+
+    private Optional<EstimatedRobotPose> update(
+            PhotonPipelineResult cameraResult, PoseStrategy strat) {
         Optional<EstimatedRobotPose> estimatedPose;
-        switch (strategy) {
+        switch (strat) {
             case LOWEST_AMBIGUITY:
                 estimatedPose = lowestAmbiguityStrategy(cameraResult);
                 break;
             case CLOSEST_TO_CAMERA_HEIGHT:
                 estimatedPose = closestToCameraHeightStrategy(cameraResult);
                 break;
+            case CLOSEST_TO_REFERENCE_POSE:
+                estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose);
+                break;
             case CLOSEST_TO_LAST_POSE:
                 setReferencePose(lastPose);
-            case CLOSEST_TO_REFERENCE_POSE:
                 estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose);
                 break;
             case AVERAGE_BEST_TARGETS:
                 estimatedPose = averageBestTargetsStrategy(cameraResult);
                 break;
+            case MULTI_TAG_PNP:
+                estimatedPose = multiTagPNPStrategy(cameraResult);
+                break;
             default:
                 DriverStation.reportError(
                         "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false);
@@ -230,6 +322,62 @@ public class PhotonPoseEstimator {
         return estimatedPose;
     }
 
+    private Optional<EstimatedRobotPose> multiTagPNPStrategy(PhotonPipelineResult result) {
+        // Arrays we need declared up front
+        var visCorners = new ArrayList<TargetCorner>();
+        var knownVisTags = new ArrayList<AprilTag>();
+        var fieldToCams = new ArrayList<Pose3d>();
+        var fieldToCamsAlt = new ArrayList<Pose3d>();
+
+        if (result.getTargets().size() < 2) {
+            // Run fallback strategy instead
+            return update(result, this.multiTagFallbackStrategy);
+        }
+
+        for (var target : result.getTargets()) {
+            visCorners.addAll(target.getDetectedCorners());
+
+            var tagPoseOpt = fieldTags.getTagPose(target.getFiducialId());
+            if (tagPoseOpt.isEmpty()) {
+                reportFiducialPoseError(target.getFiducialId());
+                continue;
+            }
+
+            var tagPose = tagPoseOpt.get();
+
+            // actual layout poses of visible tags -- not exposed, so have to recreate
+            knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose));
+
+            fieldToCams.add(tagPose.transformBy(target.getBestCameraToTarget().inverse()));
+            fieldToCamsAlt.add(tagPose.transformBy(target.getAlternateCameraToTarget().inverse()));
+        }
+
+        var cameraMatrixOpt = camera.getCameraMatrix();
+        var distCoeffsOpt = camera.getDistCoeffs();
+        boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
+
+        // multi-target solvePNP
+        if (hasCalibData) {
+            var cameraMatrix = cameraMatrixOpt.get();
+            var distCoeffs = distCoeffsOpt.get();
+            var pnpResults =
+                    VisionEstimation.estimateCamPosePNP(cameraMatrix, distCoeffs, visCorners, knownVisTags);
+            var best =
+                    new Pose3d()
+                            .plus(pnpResults.best) // field-to-camera
+                            .plus(robotToCamera.inverse()); // field-to-robot
+            // var alt = new Pose3d()
+            // .plus(pnpResults.alt) // field-to-camera
+            // .plus(robotToCamera.inverse()); // field-to-robot
+
+            return Optional.of(
+                    new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets()));
+        } else {
+            // TODO fallback strategy? Should we just always do solvePNP?
+            return Optional.empty();
+        }
+    }
+
     /**
      * Return the estimated position of the robot with the lowest position ambiguity from a List of
      * pipeline results.
@@ -271,7 +419,8 @@ public class PhotonPoseEstimator {
                                 .get()
                                 .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
                                 .transformBy(robotToCamera.inverse()),
-                        result.getTimestampSeconds()));
+                        result.getTimestampSeconds(),
+                        result.getTargets()));
     }
 
     /**
@@ -324,7 +473,8 @@ public class PhotonPoseEstimator {
                                         .get()
                                         .transformBy(target.getAlternateCameraToTarget().inverse())
                                         .transformBy(robotToCamera.inverse()),
-                                result.getTimestampSeconds());
+                                result.getTimestampSeconds(),
+                                result.getTargets());
             }
 
             if (bestTransformDelta < smallestHeightDifference) {
@@ -335,7 +485,8 @@ public class PhotonPoseEstimator {
                                         .get()
                                         .transformBy(target.getBestCameraToTarget().inverse())
                                         .transformBy(robotToCamera.inverse()),
-                                result.getTimestampSeconds());
+                                result.getTimestampSeconds(),
+                                result.getTargets());
             }
         }
 
@@ -396,12 +547,14 @@ public class PhotonPoseEstimator {
             if (altDifference < smallestPoseDelta) {
                 smallestPoseDelta = altDifference;
                 lowestDeltaPose =
-                        new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds());
+                        new EstimatedRobotPose(
+                                altTransformPosition, result.getTimestampSeconds(), result.getTargets());
             }
             if (bestDifference < smallestPoseDelta) {
                 smallestPoseDelta = bestDifference;
                 lowestDeltaPose =
-                        new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds());
+                        new EstimatedRobotPose(
+                                bestTransformPosition, result.getTimestampSeconds(), result.getTargets());
             }
         }
         return Optional.ofNullable(lowestDeltaPose);
@@ -443,7 +596,8 @@ public class PhotonPoseEstimator {
                                         .get()
                                         .transformBy(target.getBestCameraToTarget().inverse())
                                         .transformBy(robotToCamera.inverse()),
-                                result.getTimestampSeconds()));
+                                result.getTimestampSeconds(),
+                                result.getTargets()));
             }
 
             totalAmbiguity += 1.0 / target.getPoseAmbiguity();
@@ -474,7 +628,8 @@ public class PhotonPoseEstimator {
         }
 
         return Optional.of(
-                new EstimatedRobotPose(new Pose3d(transform, rotation), result.getTimestampSeconds()));
+                new EstimatedRobotPose(
+                        new Pose3d(transform, rotation), result.getTimestampSeconds(), result.getTargets()));
     }
 
     /**

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

@@ -24,6 +24,10 @@
 
 package org.photonvision;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import java.util.Arrays;
 import java.util.List;
@@ -52,6 +56,28 @@ public class SimPhotonCamera {
         ts.updateEntries();
     }
 
+    /**
+     * Publishes the camera intrinsics matrix. The matrix should be in the form: spotless:off
+     * fx  0   cx
+     * 0   fy  cy
+     * 0   0   1
+     * @param cameraMatrix The cam matrix
+     * spotless:on
+     */
+    public void setCameraIntrinsicsMat(Matrix<N3, N3> cameraMatrix) {
+        ts.cameraIntrinsicsPublisher.set(cameraMatrix.getData());
+    }
+
+    /**
+     * Publishes the camera distortion matrix. The matrix should be in the form [k1 k2 p1 p2 k3]. See
+     * more: https://docs.opencv.org/3.4/d4/d94/tutorial_camera_calibration.html
+     *
+     * @param distortionMat The distortion mat
+     */
+    public void setCameraDistortionMat(Matrix<N5, N1> distortionMat) {
+        ts.cameraDistortionPublisher.set(distortionMat.getData());
+    }
+
     /**
      * Constructs a Simulated PhotonCamera from the name of the camera.
      *

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

@@ -0,0 +1,64 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision.estimation;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
+
+/** Holds various helper geometries describing the relation between camera and target. */
+public class CameraTargetRelation {
+    public final Pose3d camPose;
+    public final Transform3d camToTarg;
+    public final double camToTargDist;
+    public final double camToTargDistXY;
+    public final Rotation2d camToTargYaw;
+    public final Rotation2d camToTargPitch;
+    /** Angle from the camera's relative x-axis */
+    public final Rotation2d camToTargAngle;
+
+    public final Transform3d targToCam;
+    public final Rotation2d targToCamYaw;
+    public final Rotation2d targToCamPitch;
+    /** Angle from the target's relative x-axis */
+    public final Rotation2d targToCamAngle;
+
+    public CameraTargetRelation(Pose3d cameraPose, Pose3d targetPose) {
+        this.camPose = cameraPose;
+        camToTarg = new Transform3d(cameraPose, targetPose);
+        camToTargDist = camToTarg.getTranslation().getNorm();
+        camToTargDistXY =
+                Math.hypot(camToTarg.getTranslation().getX(), camToTarg.getTranslation().getY());
+        camToTargYaw = new Rotation2d(camToTarg.getX(), camToTarg.getY());
+        camToTargPitch = new Rotation2d(camToTargDistXY, -camToTarg.getZ());
+        camToTargAngle =
+                new Rotation2d(Math.hypot(camToTargYaw.getRadians(), camToTargPitch.getRadians()));
+        targToCam = new Transform3d(targetPose, cameraPose);
+        targToCamYaw = new Rotation2d(targToCam.getX(), targToCam.getY());
+        targToCamPitch = new Rotation2d(camToTargDistXY, -targToCam.getZ());
+        targToCamAngle =
+                new Rotation2d(Math.hypot(targToCamYaw.getRadians(), targToCamPitch.getRadians()));
+    }
+}

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

@@ -0,0 +1,518 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision.estimation;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.CoordinateSystem;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.*;
+import edu.wpi.first.util.RuntimeLoader;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import org.ejml.simple.SimpleMatrix;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.Core;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.MatOfInt;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point;
+import org.opencv.core.Point3;
+import org.opencv.core.Rect;
+import org.opencv.core.RotatedRect;
+import org.opencv.imgproc.Imgproc;
+import org.photonvision.targeting.TargetCorner;
+
+public final class OpenCVHelp {
+    static {
+        try {
+            var loader =
+                    new RuntimeLoader<>(
+                            Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
+            loader.loadLibrary();
+        } catch (Exception e) {
+            throw new RuntimeException("Failed to load native libraries!", e);
+        }
+    }
+
+    public static MatOfDouble matrixToMat(SimpleMatrix matrix) {
+        var mat = new Mat(matrix.numRows(), matrix.numCols(), CvType.CV_64F);
+        mat.put(0, 0, matrix.getDDRM().getData());
+        var wrappedMat = new MatOfDouble();
+        mat.convertTo(wrappedMat, CvType.CV_64F);
+        mat.release();
+        return wrappedMat;
+    }
+
+    public static Matrix<Num, Num> matToMatrix(Mat mat) {
+        double[] data = new double[(int) mat.total() * mat.channels()];
+        var doubleMat = new Mat(mat.rows(), mat.cols(), CvType.CV_64F);
+        mat.convertTo(doubleMat, CvType.CV_64F);
+        doubleMat.get(0, 0, data);
+        return new Matrix<>(new SimpleMatrix(mat.rows(), mat.cols(), true, data));
+    }
+
+    /**
+     * Creates a new {@link MatOfPoint3f} with these 3d translations. The opencv tvec is a vector with
+     * three elements representing {x, y, z} in the EDN coordinate system.
+     *
+     * @param translations The translations to convert into a MatOfPoint3f
+     */
+    public static MatOfPoint3f translationToTvec(Translation3d... translations) {
+        Point3[] points = new Point3[translations.length];
+        for (int i = 0; i < translations.length; i++) {
+            var trl =
+                    CoordinateSystem.convert(translations[i], CoordinateSystem.NWU(), CoordinateSystem.EDN());
+            points[i] = new Point3(trl.getX(), trl.getY(), trl.getZ());
+        }
+        return new MatOfPoint3f(points);
+    }
+    /**
+     * Returns a new 3d translation from this {@link Mat}. The opencv tvec is a vector with three
+     * elements representing {x, y, z} in the EDN coordinate system.
+     *
+     * @param tvecInput The tvec to create a Translation3d from
+     */
+    public static Translation3d tvecToTranslation(Mat tvecInput) {
+        float[] data = new float[3];
+        var wrapped = new Mat(tvecInput.rows(), tvecInput.cols(), CvType.CV_32F);
+        tvecInput.convertTo(wrapped, CvType.CV_32F);
+        wrapped.get(0, 0, data);
+        wrapped.release();
+        return CoordinateSystem.convert(
+                new Translation3d(data[0], data[1], data[2]),
+                CoordinateSystem.EDN(),
+                CoordinateSystem.NWU());
+    }
+
+    /**
+     * Creates a new {@link MatOfPoint3f} with this 3d rotation. The opencv rvec Mat is a vector with
+     * three elements representing the axis scaled by the angle in the EDN coordinate system. (angle =
+     * norm, and axis = rvec / norm)
+     *
+     * @param rotation The rotation to convert into a MatOfPoint3f
+     */
+    public static MatOfPoint3f rotationToRvec(Rotation3d rotation) {
+        rotation = rotationNWUtoEDN(rotation);
+        return new MatOfPoint3f(new Point3(rotation.getQuaternion().toRotationVector().getData()));
+    }
+    /**
+     * Returns a 3d rotation from this {@link Mat}. The opencv rvec Mat is a vector with three
+     * elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm,
+     * and axis = rvec / norm)
+     *
+     * @param rvecInput The rvec to create a Rotation3d from
+     */
+    public static Rotation3d rvecToRotation(Mat rvecInput) {
+        float[] data = new float[3];
+        var wrapped = new Mat(rvecInput.rows(), rvecInput.cols(), CvType.CV_32F);
+        rvecInput.convertTo(wrapped, CvType.CV_32F);
+        wrapped.get(0, 0, data);
+        wrapped.release();
+        Vector<N3> axis = new Vector<>(Nat.N3());
+        axis.set(0, 0, data[0]);
+        axis.set(1, 0, data[1]);
+        axis.set(2, 0, data[2]);
+        return rotationEDNtoNWU(new Rotation3d(axis.div(axis.norm()), axis.norm()));
+    }
+
+    public static TargetCorner averageCorner(List<TargetCorner> corners) {
+        if (corners == null || corners.size() == 0) return null;
+
+        var pointMat = targetCornersToMat(corners);
+        Core.reduce(pointMat, pointMat, 0, Core.REDUCE_AVG);
+        var avgPt = matToTargetCorners(pointMat)[0];
+        pointMat.release();
+        return avgPt;
+    }
+
+    public static MatOfPoint2f targetCornersToMat(List<TargetCorner> corners) {
+        return targetCornersToMat(corners.toArray(TargetCorner[]::new));
+    }
+
+    public static MatOfPoint2f targetCornersToMat(TargetCorner... corners) {
+        var points = new Point[corners.length];
+        for (int i = 0; i < corners.length; i++) {
+            points[i] = new Point(corners[i].x, corners[i].y);
+        }
+        return new MatOfPoint2f(points);
+    }
+
+    public static TargetCorner[] pointsToTargetCorners(Point... points) {
+        var corners = new TargetCorner[points.length];
+        for (int i = 0; i < points.length; i++) {
+            corners[i] = new TargetCorner(points[i].x, points[i].y);
+        }
+        return corners;
+    }
+
+    public static TargetCorner[] matToTargetCorners(MatOfPoint2f matInput) {
+        var corners = new TargetCorner[(int) matInput.total()];
+        float[] data = new float[(int) matInput.total() * matInput.channels()];
+        matInput.get(0, 0, data);
+        for (int i = 0; i < corners.length; i++) {
+            corners[i] = new TargetCorner(data[0 + 2 * i], data[1 + 2 * i]);
+        }
+        return corners;
+    }
+
+    /**
+     * Reorders the list, optionally indexing backwards and wrapping around to the last element after
+     * the first, and shifting all indices in the direction of indexing.
+     *
+     * <p>e.g.
+     *
+     * <p>({1,2,3}, false, 1) == {2,3,1}
+     *
+     * <p>({1,2,3}, true, 0) == {1,3,2}
+     *
+     * <p>({1,2,3}, true, 1) == {3,2,1}
+     *
+     * @param <T> Element type
+     * @param elements
+     * @param backwards If indexing should happen in reverse (0, size-1, size-2, ...)
+     * @param shiftStart How much the inital index should be shifted (instead of starting at index 0,
+     *     start at shiftStart, negated if backwards)
+     * @return Reordered list
+     */
+    public static <T> List<T> reorderCircular(List<T> elements, boolean backwards, int shiftStart) {
+        int size = elements.size();
+        int dir = backwards ? -1 : 1;
+        var reordered = new ArrayList<>(elements);
+        for (int i = 0; i < size; i++) {
+            int index = (i * dir + shiftStart * dir) % size;
+            if (index < 0) index = size + index;
+            reordered.set(i, elements.get(index));
+        }
+        return reordered;
+    }
+
+    /**
+     * Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN,
+     * this would be XYZ {0, -1, 0} in NWU.
+     */
+    private static Rotation3d rotationEDNtoNWU(Rotation3d rot) {
+        return CoordinateSystem.convert(
+                        new Rotation3d(), CoordinateSystem.NWU(), CoordinateSystem.EDN())
+                .plus(CoordinateSystem.convert(rot, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
+    }
+    /**
+     * Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN,
+     * this would be XYZ {0, -1, 0} in NWU.
+     */
+    private static Rotation3d rotationNWUtoEDN(Rotation3d rot) {
+        return CoordinateSystem.convert(
+                        new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU())
+                .plus(CoordinateSystem.convert(rot, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
+    }
+
+    /**
+     * Project object points from the 3d world into the 2d camera image. The camera
+     * properties(intrinsics, distortion) determine the results of this projection.
+     *
+     * @param cameraMatrix the camera intrinsics matrix in standard opencv form
+     * @param distCoeffs the camera distortion matrix in standard opencv form
+     * @param camPose The current camera pose in the 3d world
+     * @param objectTranslations The 3d points to be projected
+     * @return The 2d points in pixels which correspond to the image of the 3d points on the camera
+     */
+    public static List<TargetCorner> projectPoints(
+            Matrix<N3, N3> cameraMatrix,
+            Matrix<N5, N1> distCoeffs,
+            Pose3d camPose,
+            List<Translation3d> objectTranslations) {
+        // translate to opencv classes
+        var objectPoints = translationToTvec(objectTranslations.toArray(new Translation3d[0]));
+        // opencv rvec/tvec describe a change in basis from world to camera
+        var basisChange = RotTrlTransform3d.makeRelativeTo(camPose);
+        var rvec = rotationToRvec(basisChange.getRotation());
+        var tvec = translationToTvec(basisChange.getTranslation());
+        var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
+        var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
+        var imagePoints = new MatOfPoint2f();
+        // project to 2d
+        Calib3d.projectPoints(objectPoints, rvec, tvec, cameraMatrixMat, distCoeffsMat, imagePoints);
+
+        // turn 2d point Mat into TargetCorners
+        var corners = matToTargetCorners(imagePoints);
+
+        // release our Mats from native memory
+        objectPoints.release();
+        rvec.release();
+        tvec.release();
+        cameraMatrixMat.release();
+        distCoeffsMat.release();
+        imagePoints.release();
+
+        return Arrays.asList(corners);
+    }
+
+    /**
+     * Undistort 2d image points using a given camera's intrinsics and distortion.
+     *
+     * <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).
+     *
+     * @param cameraMatrix the camera intrinsics matrix in standard opencv form
+     * @param distCoeffs the camera distortion matrix in standard opencv form
+     * @param corners The distorted image points
+     * @return The undistorted image points
+     */
+    public static List<TargetCorner> undistortPoints(
+            Matrix<N3, N3> cameraMatrix, Matrix<N5, N1> distCoeffs, List<TargetCorner> corners) {
+        var points_in = targetCornersToMat(corners);
+        var points_out = new MatOfPoint2f();
+        var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
+        var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
+
+        Calib3d.undistortImagePoints(points_in, points_out, cameraMatrixMat, distCoeffsMat);
+        var corners_out = matToTargetCorners(points_out);
+
+        points_in.release();
+        points_out.release();
+        cameraMatrixMat.release();
+        distCoeffsMat.release();
+
+        return Arrays.asList(corners_out);
+    }
+
+    /**
+     * Gets the (upright) rectangle which bounds this contour.
+     *
+     * <p>Note that rectangle size and position are stored with ints and do not have sub-pixel
+     * accuracy.
+     *
+     * @param corners The corners/points to be bounded
+     * @return Rectangle bounding the given corners
+     */
+    public static Rect getBoundingRect(List<TargetCorner> corners) {
+        var corn = targetCornersToMat(corners);
+        var rect = Imgproc.boundingRect(corn);
+        corn.release();
+        return rect;
+    }
+    /**
+     * Gets the rotated rectangle with minimum area which bounds this contour.
+     *
+     * <p>Note that rectangle size and position are stored with doubles and have sub-pixel accuracy.
+     *
+     * @param corners The corners/points to be bounded
+     * @return Rotated rectangle bounding the given corners
+     */
+    public static RotatedRect getMinAreaRect(List<TargetCorner> corners) {
+        var corn = targetCornersToMat(corners);
+        var rect = Imgproc.minAreaRect(corn);
+        corn.release();
+        return rect;
+    }
+    /**
+     * Get the area in pixels of this target's contour. It's important to note that this may be
+     * different from the area of the bounding rectangle around the contour.
+     *
+     * @param corners The corners defining this contour
+     * @return Area in pixels (units of corner x/y)
+     */
+    public static double getContourAreaPx(List<TargetCorner> corners) {
+        var temp = targetCornersToMat(corners);
+        var corn = new MatOfPoint(temp.toArray());
+        temp.release();
+
+        // outputHull gives us indices (of corn) that make a convex hull contour
+        var outputHull = new MatOfInt();
+        Imgproc.convexHull(corn, outputHull);
+        int[] indices = outputHull.toArray();
+        outputHull.release();
+        var tempPoints = corn.toArray();
+        var points = tempPoints.clone();
+        for (int i = 0; i < indices.length; i++) {
+            points[i] = tempPoints[indices[i]];
+        }
+        corn.fromArray(points);
+        // calculate area of the (convex hull) contour
+        double area = Imgproc.contourArea(corn);
+        corn.release();
+        return area;
+    }
+
+    /**
+     * Finds the transformation(s) that map the camera's pose to the target pose. The camera's pose
+     * relative to the target is determined by the supplied 3d points of the target's model and their
+     * associated 2d points imaged by the camera.
+     *
+     * <p>For planar targets, there may be an alternate solution which is plausible given the 2d image
+     * points. This has an associated "ambiguity" which describes the ratio of reprojection error
+     * between the "best" and "alternate" solution.
+     *
+     * <p>This method is intended for use with individual AprilTags, and will not work unless 4 points
+     * are provided.
+     *
+     * @param cameraMatrix the camera intrinsics matrix in standard opencv form
+     * @param distCoeffs the camera distortion matrix in standard opencv form
+     * @param modelTrls The translations of the object corners. These should have the object pose as
+     *     their origin. These must come in a specific, pose-relative order (in NWU):
+     *     <ul>
+     *       <li>Point 0: [0, -squareLength / 2, squareLength / 2]
+     *       <li>Point 1: [0, squareLength / 2, squareLength / 2]
+     *       <li>Point 2: [0, squareLength / 2, -squareLength / 2]
+     *       <li>Point 3: [0, -squareLength / 2, -squareLength / 2]
+     *     </ul>
+     *
+     * @param imageCorners The projection of these 3d object points into the 2d camera image. The
+     *     order should match the given object point translations.
+     * @return The resulting transformation that maps the camera pose to the target pose and the
+     *     ambiguity if an alternate solution is available.
+     */
+    public static PNPResults solvePNP_SQUARE(
+            Matrix<N3, N3> cameraMatrix,
+            Matrix<N5, N1> distCoeffs,
+            List<Translation3d> modelTrls,
+            List<TargetCorner> imageCorners) {
+        // IPPE_SQUARE expects our corners in a specific order
+        modelTrls = reorderCircular(modelTrls, true, -1);
+        imageCorners = reorderCircular(imageCorners, true, -1);
+        // translate to opencv classes
+        var objectPoints = translationToTvec(modelTrls.toArray(new Translation3d[0]));
+        var imagePoints = targetCornersToMat(imageCorners);
+        var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
+        var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
+        var rvecs = new ArrayList<Mat>();
+        var tvecs = new ArrayList<Mat>();
+        var rvec = Mat.zeros(3, 1, CvType.CV_32F);
+        var tvec = Mat.zeros(3, 1, CvType.CV_32F);
+        var reprojectionError = new Mat();
+        // calc rvecs/tvecs and associated reprojection error from image points
+        Calib3d.solvePnPGeneric(
+                objectPoints,
+                imagePoints,
+                cameraMatrixMat,
+                distCoeffsMat,
+                rvecs,
+                tvecs,
+                false,
+                Calib3d.SOLVEPNP_IPPE_SQUARE,
+                rvec,
+                tvec,
+                reprojectionError);
+
+        float[] errors = new float[2];
+        reprojectionError.get(0, 0, errors);
+        // convert to wpilib coordinates
+        var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
+
+        Transform3d alt = null;
+        if (tvecs.size() > 1) {
+            alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1)));
+        }
+
+        // release our Mats from native memory
+        objectPoints.release();
+        imagePoints.release();
+        cameraMatrixMat.release();
+        distCoeffsMat.release();
+        for (var v : rvecs) v.release();
+        for (var v : tvecs) v.release();
+        rvec.release();
+        tvec.release();
+        reprojectionError.release();
+
+        if (alt != null) return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]);
+        else return new PNPResults(best, errors[0]);
+    }
+    /**
+     * Finds the transformation that maps the camera's pose to the target pose. The camera's pose
+     * relative to the target is determined by the supplied 3d points of the target's model and their
+     * associated 2d points imaged by the camera.
+     *
+     * <p>This method is intended for use with multiple targets and has no alternate solutions. There
+     * must be at least 3 points.
+     *
+     * @param cameraMatrix the camera intrinsics matrix in standard opencv form
+     * @param distCoeffs the camera distortion matrix in standard opencv form
+     * @param objectTrls The translations of the object corners, relative to the field.
+     * @param imageCorners The projection of these 3d object points into the 2d camera image. The
+     *     order should match the given object point translations.
+     * @return The resulting transformation that maps the camera pose to the target pose. If the 3d
+     *     model points are supplied relative to the origin, this transformation brings the camera to
+     *     the origin.
+     */
+    public static PNPResults solvePNP_SQPNP(
+            Matrix<N3, N3> cameraMatrix,
+            Matrix<N5, N1> distCoeffs,
+            List<Translation3d> objectTrls,
+            List<TargetCorner> imageCorners) {
+        // translate to opencv classes
+        var objectPoints = translationToTvec(objectTrls.toArray(new Translation3d[0]));
+        var imagePoints = targetCornersToMat(imageCorners);
+        var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
+        var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
+        var rvecs = new ArrayList<Mat>();
+        var tvecs = new ArrayList<Mat>();
+        var rvec = Mat.zeros(3, 1, CvType.CV_32F);
+        var tvec = Mat.zeros(3, 1, CvType.CV_32F);
+        var reprojectionError = new Mat();
+        // calc rvec/tvec from image points
+        Calib3d.solvePnPGeneric(
+                objectPoints,
+                imagePoints,
+                cameraMatrixMat,
+                distCoeffsMat,
+                rvecs,
+                tvecs,
+                false,
+                Calib3d.SOLVEPNP_SQPNP,
+                rvec,
+                tvec,
+                reprojectionError);
+
+        float[] error = new float[1];
+        reprojectionError.get(0, 0, error);
+        // convert to wpilib coordinates
+        var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
+
+        // release our Mats from native memory
+        objectPoints.release();
+        imagePoints.release();
+        cameraMatrixMat.release();
+        distCoeffsMat.release();
+        for (var v : rvecs) v.release();
+        for (var v : tvecs) v.release();
+        rvec.release();
+        tvec.release();
+        reprojectionError.release();
+
+        return new PNPResults(best, error[0]);
+    }
+}

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

@@ -0,0 +1,72 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision.estimation;
+
+import edu.wpi.first.math.geometry.Transform3d;
+
+/**
+ * The best estimated transformation from solvePnP, and possibly an alternate transformation
+ * depending on the solvePNP method. If an alternate solution is present, the ambiguity value
+ * represents the ratio of reprojection error in the best solution to the alternate (best /
+ * alternate).
+ *
+ * <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
+ * method.
+ */
+public class PNPResults {
+    public final Transform3d best;
+    public final double bestReprojErr;
+
+    /**
+     * Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
+     * to the best solution.
+     */
+    public final Transform3d alt;
+    /** If no alternate solution is found, this is bestReprojErr */
+    public final double altReprojErr;
+    /** If no alternate solution is found, this is 0 */
+    public final double ambiguity;
+
+    public PNPResults() {
+        this(new Transform3d(), new Transform3d(), 0, 0, 0);
+    }
+
+    public PNPResults(Transform3d best, double bestReprojErr) {
+        this(best, best, 0, bestReprojErr, bestReprojErr);
+    }
+
+    public PNPResults(
+            Transform3d best,
+            Transform3d alt,
+            double ambiguity,
+            double bestReprojErr,
+            double altReprojErr) {
+        this.best = best;
+        this.alt = alt;
+        this.ambiguity = ambiguity;
+        this.bestReprojErr = bestReprojErr;
+        this.altReprojErr = altReprojErr;
+    }
+}

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

@@ -0,0 +1,114 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision.estimation;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.util.List;
+import java.util.stream.Collectors;
+
+/**
+ * Represents a transformation that first rotates a pose around the origin, and then translates it.
+ */
+public class RotTrlTransform3d {
+    private final Translation3d trl;
+    private final Rotation3d rot;
+
+    public RotTrlTransform3d() {
+        this(new Rotation3d(), new Translation3d());
+    }
+    /**
+     * Creates a rotation-translation transformation from a Transform3d.
+     *
+     * <p>Applying this transformation to poses will preserve their current origin-to-pose transform
+     * as if the origin was transformed by these components.
+     *
+     * @param trf The origin transformation
+     */
+    public RotTrlTransform3d(Transform3d trf) {
+        this(trf.getRotation(), trf.getTranslation());
+    }
+    /**
+     * A rotation-translation transformation.
+     *
+     * <p>Applying this transformation to poses will preserve their current origin-to-pose transform
+     * as if the origin was transformed by these components.
+     *
+     * @param rot The rotation component
+     * @param trl The translation component
+     */
+    public RotTrlTransform3d(Rotation3d rot, Translation3d trl) {
+        this.rot = rot;
+        this.trl = trl;
+    }
+    /**
+     * The rotation-translation transformation that makes poses in the world consider this pose as the
+     * new origin, or change the basis to this pose.
+     *
+     * @param pose The new origin
+     */
+    public static RotTrlTransform3d makeRelativeTo(Pose3d pose) {
+        return new RotTrlTransform3d(pose.getRotation(), pose.getTranslation()).inverse();
+    }
+
+    /** The inverse of this transformation. Applying the inverse will "undo" this transformation. */
+    public RotTrlTransform3d inverse() {
+        var inverseRot = rot.unaryMinus();
+        var inverseTrl = trl.rotateBy(inverseRot).unaryMinus();
+        return new RotTrlTransform3d(inverseRot, inverseTrl);
+    }
+
+    /** This transformation as a Transform3d (as if of the origin) */
+    public Transform3d getTransform() {
+        return new Transform3d(trl, rot);
+    }
+    /** The translation component of this transformation */
+    public Translation3d getTranslation() {
+        return trl;
+    }
+    /** The rotation component of this transformation */
+    public Rotation3d getRotation() {
+        return rot;
+    }
+
+    public Translation3d apply(Translation3d trl) {
+        return apply(new Pose3d(trl, new Rotation3d())).getTranslation();
+    }
+    ;
+
+    public List<Translation3d> applyTrls(List<Translation3d> trls) {
+        return trls.stream().map(t -> apply(t)).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());
+    }
+}

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

@@ -0,0 +1,116 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision.estimation;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.stream.Collectors;
+
+/** Describes the 3d model of a target. */
+public class TargetModel {
+    /**
+     * Translations of this target's vertices relative to its pose. If this target is spherical, this
+     * list has one translation with x == radius.
+     */
+    public final List<Translation3d> vertices;
+
+    public final boolean isPlanar;
+    public final boolean isSpherical;
+
+    public static final TargetModel kTag16h5 =
+            new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6));
+
+    /** Creates a rectangular, planar target model given the width and height. */
+    public TargetModel(double widthMeters, double heightMeters) {
+        this.vertices =
+                List.of(
+                        // this order is relevant for AprilTag compatibility
+                        new Translation3d(0, -widthMeters / 2.0, -heightMeters / 2.0),
+                        new Translation3d(0, widthMeters / 2.0, -heightMeters / 2.0),
+                        new Translation3d(0, widthMeters / 2.0, heightMeters / 2.0),
+                        new Translation3d(0, -widthMeters / 2.0, heightMeters / 2.0));
+        this.isPlanar = true;
+        this.isSpherical = false;
+    }
+
+    /**
+     * Creates a spherical target model which has similar dimensions when viewed from any angle. This
+     * model will only have one vertex which has x == radius.
+     */
+    public TargetModel(double diameterMeters) {
+        this.vertices = List.of(new Translation3d(diameterMeters / 2.0, 0, 0));
+        this.isPlanar = false;
+        this.isSpherical = true;
+    }
+
+    /**
+     * Creates a target model from arbitrary 3d vertices. Automatically determines if the given
+     * vertices are planar(x == 0). More than 2 vertices must be given.
+     *
+     * @param vertices Translations representing the vertices of this target model relative to its
+     *     pose.
+     */
+    public TargetModel(List<Translation3d> vertices) {
+        this.isSpherical = false;
+        if (vertices == null || vertices.size() <= 2) {
+            vertices = new ArrayList<>();
+            this.isPlanar = false;
+        } else {
+            boolean cornersPlanar = true;
+            for (Translation3d corner : vertices) {
+                if (corner.getX() != 0) cornersPlanar = false;
+            }
+            this.isPlanar = cornersPlanar;
+        }
+        this.vertices = vertices;
+    }
+
+    /**
+     * This target's vertices offset from its field pose.
+     *
+     * <p>Note: If this target is spherical, only one vertex radius meters in front of the pose is
+     * returned.
+     */
+    public List<Translation3d> getFieldVertices(Pose3d targetPose) {
+        return vertices.stream()
+                .map(t -> targetPose.plus(new Transform3d(t, new Rotation3d())).getTranslation())
+                .collect(Collectors.toList());
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj instanceof TargetModel) {
+            var o = (TargetModel) obj;
+            return vertices.equals(o.vertices) && isPlanar == o.isPlanar && isSpherical == o.isSpherical;
+        }
+        return false;
+    }
+}

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

@@ -0,0 +1,168 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision.estimation;
+
+import edu.wpi.first.apriltag.AprilTag;
+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 org.photonvision.targeting.TargetCorner;
+
+public class VisionEstimation {
+    public static final TargetModel kTagModel =
+            new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6));
+
+    /**
+     * 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 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) {
+            return new PNPResults();
+        }
+        // single-tag pnp
+        if (corners.size() == 4) {
+            var camToTag =
+                    OpenCVHelp.solvePNP_SQUARE(
+                            cameraMatrix, distCoeffs, kTagModel.getFieldVertices(knownTags.get(0).pose), corners);
+            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),
+                    new Transform3d(o, altPose),
+                    camToTag.ambiguity,
+                    camToTag.bestReprojErr,
+                    camToTag.altReprojErr);
+        }
+        // multi-tag pnp
+        else {
+            var objectTrls = new ArrayList<Translation3d>();
+            for (var tag : knownTags) objectTrls.addAll(kTagModel.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(),
+                    camToOrigin.ambiguity,
+                    camToOrigin.bestReprojErr,
+                    camToOrigin.altReprojErr);
+        }
+    }
+
+    // /**
+    //  * The best estimated transformation to the target, and possibly an alternate
+    //  * transformation
+    //  * depending on the solvePNP method. If an alternate solution is present, the
+    //  * ambiguity value
+    //  * represents the ratio of reprojection error in the best solution to the
+    //  * alternate (best / alternate).
+    //  */
+    // public static class PNPResults {
+    //     public final Transform3d best;
+    //     public final double bestReprojErr;
+
+    //     /**
+    //      * Alternate, ambiguous solution from solvepnp. This may be empty
+    //      * if no alternate solution is found.
+    //      */
+    //     public final Transform3d alt;
+    //     /** If no alternate solution is found, this is 0 */
+    //     public final double altReprojErr;
+
+    //     /** If no alternate solution is found, this is 0 */
+    //     public final double ambiguity;
+
+    //     public PNPResults() {
+    //         this(new Transform3d(), new Transform3d(), 0, 0, 0);
+    //     }
+
+    //     public PNPResults(
+    //             Transform3d best, Transform3d alt,
+    //             double ambiguity, double bestReprojErr, double altReprojErr) {
+    //         this.best = best;
+    //         this.alt = alt;
+    //         this.ambiguity = ambiguity;
+    //         this.bestReprojErr = bestReprojErr;
+    //         this.altReprojErr = 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;
+        }
+    }
+}

photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp

@@ -26,6 +26,7 @@
 
 #include <frc/Errors.h>
 #include <frc/Timer.h>
+#include <opencv2/core/mat.hpp>
 
 #include "PhotonVersion.h"
 #include "photonlib/Packet.h"
@@ -50,16 +51,21 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
           rootTable->GetIntegerTopic("outputSaveImgCmd").Publish()),
       outputSaveImgSubscriber(
           rootTable->GetIntegerTopic("outputSaveImgCmd").Subscribe(0)),
-      pipelineIndexEntry(rootTable->GetIntegerTopic("pipelineIndex").Publish()),
-      ledModeEntry(mainTable->GetIntegerTopic("ledMode").Publish()),
+      pipelineIndexPub(
+          rootTable->GetIntegerTopic("pipelineIndexRequest").Publish()),
+      pipelineIndexSub(
+          rootTable->GetIntegerTopic("pipelineIndexState").Subscribe(0)),
+      ledModePub(mainTable->GetIntegerTopic("ledMode").Publish()),
+      ledModeSub(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
       versionEntry(mainTable->GetStringTopic("version").Subscribe("")),
+      cameraIntrinsicsSubscriber(
+          rootTable->GetDoubleArrayTopic("cameraIntrinsics").Subscribe({})),
+      cameraDistortionSubscriber(
+          rootTable->GetDoubleArrayTopic("cameraDistortion").Subscribe({})),
       driverModeSubscriber(
           rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
       driverModePublisher(
           rootTable->GetBooleanTopic("driverModeRequest").Publish()),
-      pipelineIndexSubscriber(
-          rootTable->GetIntegerTopic("pipelineIndex").Subscribe(-1)),
-      ledModeSubscriber(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
       m_topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}),
       path(rootTable->GetPath()),
       m_cameraName(cameraName) {}
@@ -68,7 +74,10 @@ PhotonCamera::PhotonCamera(const std::string_view cameraName)
     : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
 
 PhotonPipelineResult PhotonCamera::GetLatestResult() {
-  if (test) return testResult;
+  if (test) {
+    return testResult;
+  }
+
   // Prints warning if not connected
   VerifyVersion();
 
@@ -107,23 +116,37 @@ void PhotonCamera::TakeOutputSnapshot() {
 bool PhotonCamera::GetDriverMode() const { return driverModeSubscriber.Get(); }
 
 void PhotonCamera::SetPipelineIndex(int index) {
-  pipelineIndexEntry.Set(static_cast<double>(index));
+  pipelineIndexPub.Set(static_cast<double>(index));
 }
 
 int PhotonCamera::GetPipelineIndex() const {
-  return static_cast<int>(pipelineIndexSubscriber.Get());
+  return static_cast<int>(pipelineIndexSub.Get());
 }
 
 LEDMode PhotonCamera::GetLEDMode() const {
-  return static_cast<LEDMode>(static_cast<int>(ledModeSubscriber.Get()));
+  return static_cast<LEDMode>(static_cast<int>(ledModeSub.Get()));
+}
+
+std::optional<cv::Mat> PhotonCamera::GetCameraMatrix() {
+  auto camCoeffs = cameraIntrinsicsSubscriber.Get();
+  if (camCoeffs.size() == 9) {
+    // clone should deal with ownership concerns? not sure
+    return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone();
+  }
+  return std::nullopt;
 }
 
 void PhotonCamera::SetLEDMode(LEDMode mode) {
-  ledModeEntry.Set(static_cast<double>(static_cast<int>(mode)));
+  ledModePub.Set(static_cast<double>(static_cast<int>(mode)));
 }
 
-const std::string_view PhotonCamera::GetCameraName() const {
-  return m_cameraName;
+std::optional<cv::Mat> PhotonCamera::GetDistCoeffs() {
+  auto distCoeffs = cameraDistortionSubscriber.Get();
+  if (distCoeffs.size() == 5) {
+    // clone should deal with ownership concerns? not sure
+    return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone();
+  }
+  return std::nullopt;
 }
 
 void PhotonCamera::VerifyVersion() {

photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp

@@ -24,6 +24,7 @@
 
 #include "photonlib/PhotonPoseEstimator.h"
 
+#include <cmath>
 #include <iostream>
 #include <limits>
 #include <map>
@@ -36,6 +37,10 @@
 #include <frc/geometry/Pose3d.h>
 #include <frc/geometry/Rotation3d.h>
 #include <frc/geometry/Transform3d.h>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core/mat.hpp>
+#include <opencv2/core/types.hpp>
+#include <units/math.h>
 #include <units/time.h>
 
 #include "photonlib/PhotonCamera.h"
@@ -43,6 +48,16 @@
 #include "photonlib/PhotonTrackedTarget.h"
 
 namespace photonlib {
+
+namespace detail {
+cv::Point3d ToPoint3d(const frc::Translation3d& translation);
+std::optional<std::array<cv::Point3d, 4>> CalcTagCorners(
+    int tagID, const frc::AprilTagFieldLayout& aprilTags);
+frc::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec);
+cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
+                                   units::meter_t cornerY, frc::Pose3d tagPose);
+}  // namespace detail
+
 PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
                                          PoseStrategy strat, PhotonCamera&& cam,
                                          frc::Transform3d robotToCamera)
@@ -51,15 +66,55 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
       camera(std::move(cam)),
       m_robotToCamera(robotToCamera),
       lastPose(frc::Pose3d()),
-      referencePose(frc::Pose3d()) {}
+      referencePose(frc::Pose3d()),
+      poseCacheTimestamp(-1_s) {}
+
+void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
+  if (strategy == MULTI_TAG_PNP) {
+    FRC_ReportError(
+        frc::warn::Warning,
+        "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
+        "");
+    strategy = LOWEST_AMBIGUITY;
+  }
+  if (this->multiTagFallbackStrategy != strategy) {
+    InvalidatePoseCache();
+  }
+  multiTagFallbackStrategy = strategy;
+}
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
   auto result = camera.GetLatestResult();
+  return Update(result);
+}
 
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
+    const PhotonPipelineResult& result) {
+  // Time in the past -- give up, since the following if expects times > 0
+  if (result.GetTimestamp() < 0_s) {
+    return std::nullopt;
+  }
+
+  // If the pose cache timestamp was set, and the result is from the same
+  // timestamp, return an empty result
+  if (poseCacheTimestamp > 0_s &&
+      units::math::abs(poseCacheTimestamp - result.GetTimestamp()) < 0.001_ms) {
+    return std::nullopt;
+  }
+
+  // Remember the timestamp of the current result used
+  poseCacheTimestamp = result.GetTimestamp();
+
+  // If no targets seen, trivial case -- return empty result
   if (!result.HasTargets()) {
     return std::nullopt;
   }
 
+  return Update(result, this->strategy);
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
+    PhotonPipelineResult result, PoseStrategy strategy) {
   std::optional<EstimatedRobotPose> ret = std::nullopt;
 
   switch (strategy) {
@@ -79,14 +134,13 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
     case AVERAGE_BEST_TARGETS:
       ret = AverageBestTargetsStrategy(result);
       break;
+    case ::photonlib::MULTI_TAG_PNP:
+      ret = MultiTagPnpStrategy(result);
+      break;
     default:
       FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
                       "");
-      return std::nullopt;
-  }
-
-  if (!ret) {
-    // TODO
+      ret = std::nullopt;
   }
 
   return ret;
@@ -94,21 +148,21 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
     PhotonPipelineResult result) {
-  int lowestAJ = -1;
   double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
   auto targets = result.GetTargets();
-  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-    if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) {
-      lowestAJ = j;
-      lowestAmbiguityScore = targets[j].GetPoseAmbiguity();
+  auto foundIt = targets.end();
+  for (auto it = targets.begin(); it != targets.end(); ++it) {
+    if (it->GetPoseAmbiguity() < lowestAmbiguityScore) {
+      foundIt = it;
+      lowestAmbiguityScore = it->GetPoseAmbiguity();
     }
   }
 
-  if (lowestAJ == -1) {
+  if (foundIt == targets.end()) {
     return std::nullopt;
   }
 
-  PhotonTrackedTarget bestTarget = targets[lowestAJ];
+  auto& bestTarget = *foundIt;
 
   std::optional<frc::Pose3d> fiducialPose =
       aprilTags.GetTagPose(bestTarget.GetFiducialId());
@@ -123,7 +177,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
       fiducialPose.value()
           .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
           .TransformBy(m_robotToCamera.Inverse()),
-      result.GetTimestamp()};
+      result.GetTimestamp(), result.GetTargets()};
 }
 
 std::optional<EstimatedRobotPose>
@@ -143,14 +197,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
                       target.GetFiducialId());
       continue;
     }
-    frc::Pose3d targetPose = fiducialPose.value();
+    frc::Pose3d const targetPose = fiducialPose.value();
 
-    units::meter_t alternativeDifference = units::math::abs(
+    units::meter_t const alternativeDifference = units::math::abs(
         m_robotToCamera.Z() -
         targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
             .Z());
 
-    units::meter_t bestDifference = units::math::abs(
+    units::meter_t const bestDifference = units::math::abs(
         m_robotToCamera.Z() -
         targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
 
@@ -159,14 +213,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
       pose = EstimatedRobotPose{
           targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
               .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp()};
+          result.GetTimestamp(), result.GetTargets()};
     }
     if (bestDifference < smallestHeightDifference) {
       smallestHeightDifference = bestDifference;
       pose = EstimatedRobotPose{
           targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
               .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp()};
+          result.GetTimestamp(), result.GetTargets()};
     }
   }
 
@@ -182,8 +236,7 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
   frc::Pose3d pose = lastPose;
 
   auto targets = result.GetTargets();
-  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-    PhotonTrackedTarget target = targets[j];
+  for (auto& target : targets) {
     std::optional<frc::Pose3d> fiducialPose =
         aprilTags.GetTagPose(target.GetFiducialId());
     if (!fiducialPose) {
@@ -201,9 +254,9 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
         targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
             .TransformBy(m_robotToCamera.Inverse());
 
-    units::meter_t alternativeDifference = units::math::abs(
+    units::meter_t const alternativeDifference = units::math::abs(
         referencePose.Translation().Distance(altPose.Translation()));
-    units::meter_t bestDifference = units::math::abs(
+    units::meter_t const bestDifference = units::math::abs(
         referencePose.Translation().Distance(bestPose.Translation()));
     if (alternativeDifference < smallestDifference) {
       smallestDifference = alternativeDifference;
@@ -218,7 +271,113 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
     }
   }
 
-  return EstimatedRobotPose{pose, stateTimestamp};
+  return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets()};
+}
+
+std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
+    int tagID, const frc::AprilTagFieldLayout& aprilTags) {
+  if (auto tagPose = aprilTags.GetTagPose(tagID); tagPose.has_value()) {
+    return std::array{TagCornerToObjectPoint(-3_in, -3_in, *tagPose),
+                      TagCornerToObjectPoint(+3_in, -3_in, *tagPose),
+                      TagCornerToObjectPoint(+3_in, +3_in, *tagPose),
+                      TagCornerToObjectPoint(-3_in, +3_in, *tagPose)};
+  } else {
+    return std::nullopt;
+  }
+}
+
+cv::Point3d detail::ToPoint3d(const frc::Translation3d& translation) {
+  return cv::Point3d(-translation.Y().value(), -translation.Z().value(),
+                     +translation.X().value());
+}
+
+cv::Point3d detail::TagCornerToObjectPoint(units::meter_t cornerX,
+                                           units::meter_t cornerY,
+                                           frc::Pose3d tagPose) {
+  frc::Translation3d cornerTrans =
+      tagPose.Translation() +
+      frc::Translation3d(0.0_m, cornerX, cornerY).RotateBy(tagPose.Rotation());
+  return ToPoint3d(cornerTrans);
+}
+
+frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
+  using namespace frc;
+  using namespace units;
+
+  cv::Mat R;
+  cv::Rodrigues(rvec, R);  // R is 3x3
+
+  R = R.t();                  // rotation of inverse
+  cv::Mat tvecI = -R * tvec;  // translation of inverse
+
+  Vectord<3> tv;
+  tv[0] = +tvecI.at<double>(2, 0);
+  tv[1] = -tvecI.at<double>(0, 0);
+  tv[2] = -tvecI.at<double>(1, 0);
+  Vectord<3> rv;
+  rv[0] = +rvec.at<double>(2, 0);
+  rv[1] = -rvec.at<double>(0, 0);
+  rv[2] = +rvec.at<double>(1, 0);
+
+  return Pose3d(Translation3d(meter_t{tv[0]}, meter_t{tv[1]}, meter_t{tv[2]}),
+                Rotation3d(
+                    // radian_t{rv[0]},
+                    // radian_t{rv[1]},
+                    // radian_t{rv[2]}
+                    rv, radian_t{rv.norm()}));
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
+    PhotonPipelineResult result) {
+  using namespace frc;
+
+  if (!result.HasTargets() || result.GetTargets().size() < 2) {
+    return Update(result, this->multiTagFallbackStrategy);
+  }
+
+  auto const targets = result.GetTargets();
+
+  // List of corners mapped from 3d space (meters) to the 2d camera screen
+  // (pixels).
+  std::vector<cv::Point3f> objectPoints;
+  std::vector<cv::Point2f> imagePoints;
+
+  // Add all target corners to main list of corners
+  for (auto target : targets) {
+    int id = target.GetFiducialId();
+    if (auto const tagCorners = detail::CalcTagCorners(id, aprilTags);
+        tagCorners.has_value()) {
+      auto const targetCorners = target.GetDetectedCorners();
+      for (size_t cornerIdx = 0; cornerIdx < 4; ++cornerIdx) {
+        imagePoints.emplace_back(targetCorners[cornerIdx].first,
+                                 targetCorners[cornerIdx].second);
+        objectPoints.emplace_back((*tagCorners)[cornerIdx]);
+      }
+    }
+  }
+
+  if (imagePoints.empty()) {
+    return std::nullopt;
+  }
+
+  // Use OpenCV ITERATIVE solver
+  cv::Mat const rvec(3, 1, cv::DataType<double>::type);
+  cv::Mat const tvec(3, 1, cv::DataType<double>::type);
+
+  auto const camMat = camera.GetCameraMatrix();
+  auto const distCoeffs = camera.GetDistCoeffs();
+  if (!camMat || !distCoeffs) {
+    return std::nullopt;
+  }
+
+  cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(),
+               rvec, tvec, false, cv::SOLVEPNP_SQPNP);
+
+  Pose3d const pose = detail::ToPose3d(tvec, rvec);
+
+  return photonlib::EstimatedRobotPose(
+      pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
+      result.GetTargets());
 }
 
 std::optional<EstimatedRobotPose>
@@ -228,8 +387,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
   double totalAmbiguity = 0;
 
   auto targets = result.GetTargets();
-  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-    PhotonTrackedTarget target = targets[j];
+  for (auto& target : targets) {
     std::optional<frc::Pose3d> fiducialPose =
         aprilTags.GetTagPose(target.GetFiducialId());
     if (!fiducialPose) {
@@ -245,7 +403,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
       return EstimatedRobotPose{
           targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
               .TransformBy(m_robotToCamera.Inverse()),
-          result.GetLatency()};
+          result.GetTimestamp(), result.GetTargets()};
     }
     totalAmbiguity += 1. / target.GetPoseAmbiguity();
 
@@ -259,12 +417,12 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
 
   for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
        tempPoses) {
-    double weight = (1. / pair.second.first) / totalAmbiguity;
+    double const weight = (1. / pair.second.first) / totalAmbiguity;
     transform = transform + pair.first.Translation() * weight;
     rotation = rotation + pair.first.Rotation() * weight;
   }
 
   return EstimatedRobotPose{frc::Pose3d(transform, rotation),
-                            result.GetTimestamp()};
+                            result.GetTimestamp(), result.GetTargets()};
 }
 }  // namespace photonlib

photon-lib/src/main/native/include/photonlib/PhotonCamera.h

@@ -28,6 +28,7 @@
 #include <string>
 
 #include <networktables/BooleanTopic.h>
+#include <networktables/DoubleArrayTopic.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/IntegerTopic.h>
 #include <networktables/MultiSubscriber.h>
@@ -40,6 +41,10 @@
 
 #include "photonlib/PhotonPipelineResult.h"
 
+namespace cv {
+class Mat;
+}  // namespace cv
+
 namespace photonlib {
 
 enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 };
@@ -144,6 +149,9 @@ class PhotonCamera {
    */
   const std::string_view GetCameraName() const;
 
+  std::optional<cv::Mat> GetCameraMatrix();
+  std::optional<cv::Mat> GetDistCoeffs();
+
   /**
    * Returns whether the latest target result has targets.
    * This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should
@@ -172,13 +180,17 @@ class PhotonCamera {
   nt::IntegerSubscriber inputSaveImgSubscriber;
   nt::IntegerPublisher outputSaveImgEntry;
   nt::IntegerSubscriber outputSaveImgSubscriber;
-  nt::IntegerPublisher pipelineIndexEntry;
-  nt::IntegerPublisher ledModeEntry;
+  nt::IntegerPublisher pipelineIndexPub;
+  nt::IntegerSubscriber pipelineIndexSub;
+  nt::IntegerPublisher ledModePub;
+  nt::IntegerSubscriber ledModeSub;
   nt::StringSubscriber versionEntry;
 
+  nt::DoubleArraySubscriber cameraIntrinsicsSubscriber;
+  nt::DoubleArraySubscriber cameraDistortionSubscriber;
+
   nt::BooleanSubscriber driverModeSubscriber;
   nt::BooleanPublisher driverModePublisher;
-  nt::IntegerSubscriber pipelineIndexSubscriber;
   nt::IntegerSubscriber ledModeSubscriber;
 
   nt::MultiSubscriber m_topicNameSubscriber;

photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h

@@ -24,24 +24,27 @@
 
 #pragma once
 
-#include <map>
 #include <memory>
-#include <utility>
-#include <vector>
 
 #include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/geometry/Pose3d.h>
 #include <frc/geometry/Transform3d.h>
 
 #include "photonlib/PhotonCamera.h"
+#include "photonlib/PhotonPipelineResult.h"
+
+namespace cv {
+class Mat;
+}  // namespace cv
 
 namespace photonlib {
-enum PoseStrategy : int {
-  LOWEST_AMBIGUITY,
+enum PoseStrategy {
+  LOWEST_AMBIGUITY = 0,
   CLOSEST_TO_CAMERA_HEIGHT,
   CLOSEST_TO_REFERENCE_POSE,
   CLOSEST_TO_LAST_POSE,
-  AVERAGE_BEST_TARGETS
+  AVERAGE_BEST_TARGETS,
+  MULTI_TAG_PNP
 };
 
 struct EstimatedRobotPose {
@@ -51,8 +54,14 @@ struct EstimatedRobotPose {
    * the same timebase as the RoboRIO FPGA Timestamp */
   units::second_t timestamp;
 
-  EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_)
-      : estimatedPose(pose_), timestamp(time_) {}
+  /** A list of the targets used to compute this pose */
+  wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
+
+  EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
+                     std::span<const PhotonTrackedTarget> targets)
+      : estimatedPose(pose_),
+        timestamp(time_),
+        targetsUsed(targets.data(), targets.data() + targets.size()) {}
 };
 
 /**
@@ -63,10 +72,6 @@ struct EstimatedRobotPose {
  */
 class PhotonPoseEstimator {
  public:
-  using map_value_type =
-      std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
-  using size_type = std::vector<map_value_type>::size_type;
-
   /**
    * Create a new PhotonPoseEstimator.
    *
@@ -104,7 +109,20 @@ class PhotonPoseEstimator {
    *
    * @param strategy the strategy to set
    */
-  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
+  inline void SetPoseStrategy(PoseStrategy strat) {
+    if (strategy != strat) {
+      InvalidatePoseCache();
+    }
+    strategy = strat;
+  }
+
+  /**
+   * Set the Position Estimation Strategy used in multi-tag mode when
+   * only one tag can be seen. Must NOT be MULTI_TAG_PNP
+   *
+   * @param strategy the strategy to set
+   */
+  void SetMultiTagFallbackStrategy(PoseStrategy strategy);
 
   /**
    * Return the reference position that is being used by the estimator.
@@ -120,9 +138,30 @@ class PhotonPoseEstimator {
    * @param referencePose the referencePose to set
    */
   inline void SetReferencePose(frc::Pose3d referencePose) {
+    if (this->referencePose != referencePose) {
+      InvalidatePoseCache();
+    }
     this->referencePose = referencePose;
   }
 
+  /**
+   * @return The current transform from the center of the robot to the camera
+   *         mount position.
+   */
+  inline frc::Transform3d GetRobotToCameraTransform() {
+    return m_robotToCamera;
+  }
+
+  /**
+   * Useful for pan and tilt mechanisms, or cameras on turrets
+   *
+   * @param robotToCamera The current transform from the center of the robot to
+   * the camera mount position.
+   */
+  inline void SetRobotToCameraTransform(frc::Transform3d robotToCamera) {
+    m_robotToCamera = robotToCamera;
+  }
+
   /**
    * Update the stored last pose. Useful for setting the initial estimate when
    * using the CLOSEST_TO_LAST_POSE strategy.
@@ -137,11 +176,17 @@ class PhotonPoseEstimator {
    */
   std::optional<EstimatedRobotPose> Update();
 
+  /**
+   * Update the pose estimator.
+   */
+  std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result);
+
   inline PhotonCamera& GetCamera() { return camera; }
 
  private:
   frc::AprilTagFieldLayout aprilTags;
   PoseStrategy strategy;
+  PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
 
   PhotonCamera camera;
   frc::Transform3d m_robotToCamera;
@@ -149,6 +194,13 @@ class PhotonPoseEstimator {
   frc::Pose3d lastPose;
   frc::Pose3d referencePose;
 
+  units::second_t poseCacheTimestamp;
+
+  inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
+
+  std::optional<EstimatedRobotPose> Update(PhotonPipelineResult result,
+                                           PoseStrategy strategy);
+
   /**
    * Return the estimated position of the robot with the lowest position
    * ambiguity from a List of pipeline results.
@@ -182,12 +234,21 @@ class PhotonPoseEstimator {
   std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
       PhotonPipelineResult result);
 
+  /**
+   * Return the pose calculation using all targets in view in the same PNP()
+   calculation
+
+   * @return the estimated position of the robot in the FCS and the estimated
+   timestamp of this estimation.
+   */
+  std::optional<EstimatedRobotPose> MultiTagPnpStrategy(
+      PhotonPipelineResult result);
+
   /**
    * Return the average of the best target poses using ambiguity as weight.
 
    * @return the estimated position of the robot in the FCS and the estimated
-   timestamp of this
-   *     estimation.
+   timestamp of this estimation.
    */
   std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
       PhotonPipelineResult result);

photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java

@@ -25,11 +25,12 @@
 package org.photonvision;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.hal.JNIWrapper;
-import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
@@ -157,8 +158,6 @@ class PhotonPoseEstimatorTest {
 
     @Test
     void testClosestToCameraHeightStrategy() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
-
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
         cameraOne.result =
                 new PhotonPipelineResult(
@@ -387,6 +386,7 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
+        cameraOne.result.setTimestampSeconds(1);
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
@@ -473,9 +473,72 @@ class PhotonPoseEstimatorTest {
     }
 
     @Test
-    void averageBestPoses() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
+    void cacheIsInvalidated() {
+        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
+        var result =
+                new PhotonPipelineResult(
+                        2,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        0,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+        result.setTimestampSeconds(20);
 
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.AVERAGE_BEST_TARGETS,
+                        cameraOne,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
+
+        // Empty result, expect empty result
+        cameraOne.result = new PhotonPipelineResult();
+        cameraOne.result.setTimestampSeconds(1);
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        assertFalse(estimatedPose.isPresent());
+
+        // Set actual result
+        cameraOne.result = result;
+        estimatedPose = estimator.update();
+        assertTrue(estimatedPose.isPresent());
+        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+
+        // And again -- pose cache should mean this is empty
+        cameraOne.result = result;
+        estimatedPose = estimator.update();
+        assertFalse(estimatedPose.isPresent());
+        // Expect the old timestamp to still be here
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+
+        // Set new field layout -- right after, the pose cache timestamp should be -1
+        estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
+        assertEquals(-1, estimator.poseCacheTimestampSeconds);
+        // Update should cache the current timestamp (20) again
+        cameraOne.result = result;
+        estimatedPose = estimator.update();
+        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+    }
+
+    @Test
+    void averageBestPoses() {
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
         cameraOne.result =
                 new PhotonPipelineResult(

photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java

@@ -0,0 +1,101 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision.estimation;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.cscore.CameraServerCvJNI;
+import edu.wpi.first.hal.JNIWrapper;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.net.WPINetJNI;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.util.CombinedRuntimeLoader;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.io.IOException;
+import org.junit.jupiter.api.BeforeAll;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+
+public class ApriltagWorkbenchTest {
+    @BeforeAll
+    public static void setUp() {
+        JNIWrapper.Helper.setExtractOnStaticLoad(false);
+        WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
+        NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
+        WPINetJNI.Helper.setExtractOnStaticLoad(false);
+        CameraServerCvJNI.Helper.setExtractOnStaticLoad(false);
+
+        try {
+            CombinedRuntimeLoader.loadLibraries(
+                    ApriltagWorkbenchTest.class,
+                    "wpiutiljni",
+                    "ntcorejni",
+                    "wpinetjni",
+                    "wpiHaljni",
+                    "cscorejnicvstatic");
+        } catch (Exception e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+        }
+
+        // No version check for testing
+        PhotonCamera.setVersionCheckEnabled(false);
+    }
+
+    // @Test
+    public void testMeme() throws IOException, InterruptedException {
+        NetworkTableInstance instance = NetworkTableInstance.getDefault();
+        instance.stopServer();
+        // set the NT server if simulating this code.
+        // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]"
+        // for coprocessor
+        instance.setServer("localhost");
+        instance.startClient4("myRobot");
+
+        var robotToCamera = new Transform3d();
+        var cam = new PhotonCamera("WPI2023");
+        var tagLayout =
+                AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile);
+
+        var pe =
+                new PhotonPoseEstimator(
+                        tagLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, cam, robotToCamera);
+
+        var field = new Field2d();
+        SmartDashboard.putData(field);
+
+        while (!Thread.interrupted()) {
+            Thread.sleep(500);
+
+            var ret = pe.update();
+            System.out.println(ret);
+
+            field.setRobotPose(ret.get().estimatedPose.toPose2d());
+        }
+    }
+}

photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp

@@ -231,6 +231,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
   estimator.SetLastPose(
       frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
   auto estimatedPose = estimator.Update();
+  ASSERT_TRUE(estimatedPose);
   frc::Pose3d pose = estimatedPose.value().estimatedPose;
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targetsThree{
@@ -257,12 +258,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
           0.4, corners, detectedCorners}};
 
   estimator.GetCamera().testResult = {2_ms, targetsThree};
-  estimator.GetCamera().testResult.SetTimestamp(units::second_t(7));
+  estimator.GetCamera().testResult.SetTimestamp(units::second_t(21));
 
   estimatedPose = estimator.Update();
+  ASSERT_TRUE(estimatedPose);
   pose = estimatedPose.value().estimatedPose;
 
-  EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.value().timestamp),
+  EXPECT_NEAR(21.0, units::unit_cast<double>(estimatedPose.value().timestamp),
               .01);
   EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
   EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
@@ -310,3 +312,52 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
   EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
   EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
 }
+
+TEST(PhotonPoseEstimatorTest, PoseCache) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test2");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 0,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+
+  photonlib::PhotonPoseEstimator estimator(
+      aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
+
+  // empty input, expect empty out
+  estimator.GetCamera().testResult = {2_ms, {}};
+  estimator.GetCamera().testResult.SetTimestamp(units::second_t(1));
+  auto estimatedPose = estimator.Update();
+  EXPECT_FALSE(estimatedPose);
+
+  // Set result, and update -- expect present and timestamp to be 15
+  estimator.GetCamera().testResult = {3_ms, targets};
+  estimator.GetCamera().testResult.SetTimestamp(units::second_t(15));
+  estimatedPose = estimator.Update();
+  EXPECT_TRUE(estimatedPose);
+  EXPECT_NEAR(15, estimatedPose.value().timestamp.to<double>(), 1e-6);
+
+  // And again -- now pose cache should be empty
+  estimatedPose = estimator.Update();
+  EXPECT_FALSE(estimatedPose);
+}

photon-server/networktables.json

@@ -0,0 +1 @@
+[]

photon-targeting/build.gradle

@@ -5,6 +5,11 @@ apply from: "${rootDir}/shared/common.gradle"
 dependencies {
     implementation wpilibTools.deps.wpilibJava("wpimath")
     implementation "org.ejml:ejml-simple:0.41"
+
+    // Junit
+    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")
 }
 
 tasks.withType(JavaCompile) {
@@ -19,4 +24,8 @@ java {
     withSourcesJar()
 }
 
+test {
+    useJUnitPlatform()
+}
+
 apply from: "publish.gradle"

photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java

@@ -182,4 +182,18 @@ public class Packet {
         }
         return packetData[readPos++] == 1;
     }
+
+    public void encode(double[] data) {
+        for (double d : data) {
+            encode(d);
+        }
+    }
+
+    public double[] decodeDoubleArray(int len) {
+        double[] ret = new double[len];
+        for (int i = 0; i < len; i++) {
+            ret[i] = decodeDouble();
+        }
+        return ret;
+    }
 }

photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java

@@ -41,9 +41,8 @@ public class NTTopicSet {
     public NetworkTable subTable;
     public RawPublisher rawBytesEntry;
 
-    public IntegerTopic pipelineIndexTopic;
     public IntegerPublisher pipelineIndexPublisher;
-    public IntegerSubscriber pipelineIndexSubscriber;
+    public IntegerSubscriber pipelineIndexRequestSub;
 
     public BooleanTopic driverModeEntry;
     public BooleanPublisher driverModePublisher;
@@ -65,15 +64,18 @@ public class NTTopicSet {
     public IntegerTopic heartbeatTopic;
     public IntegerPublisher heartbeatPublisher;
 
+    // Camera Calibration
+    public DoubleArrayPublisher cameraIntrinsicsPublisher;
+    public DoubleArrayPublisher cameraDistortionPublisher;
+
     public void updateEntries() {
         rawBytesEntry =
                 subTable
                         .getRawTopic("rawBytes")
                         .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
 
-        pipelineIndexTopic = subTable.getIntegerTopic("pipelineIndex");
-        pipelineIndexPublisher = pipelineIndexTopic.publish();
-        pipelineIndexSubscriber = pipelineIndexTopic.subscribe(0);
+        pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish();
+        pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0);
 
         driverModePublisher = subTable.getBooleanTopic("driverMode").publish();
         driverModeSubscriber = subTable.getBooleanTopic("driverModeRequest").subscribe(false);
@@ -95,13 +97,16 @@ public class NTTopicSet {
 
         heartbeatTopic = subTable.getIntegerTopic("heartbeat");
         heartbeatPublisher = heartbeatTopic.publish();
+
+        cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish();
+        cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish();
     }
 
     @SuppressWarnings("DuplicatedCode")
     public void removeEntries() {
         if (rawBytesEntry != null) rawBytesEntry.close();
         if (pipelineIndexPublisher != null) pipelineIndexPublisher.close();
-        if (pipelineIndexSubscriber != null) pipelineIndexSubscriber.close();
+        if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close();
 
         if (driverModePublisher != null) driverModePublisher.close();
         if (driverModeSubscriber != null) driverModeSubscriber.close();
@@ -115,5 +120,10 @@ public class NTTopicSet {
         if (targetSkewEntry != null) targetSkewEntry.close();
         if (bestTargetPosX != null) bestTargetPosX.close();
         if (bestTargetPosY != null) bestTargetPosY.close();
+
+        if (heartbeatPublisher != null) heartbeatPublisher.close();
+
+        if (cameraIntrinsicsPublisher != null) cameraIntrinsicsPublisher.close();
+        if (cameraDistortionPublisher != null) cameraDistortionPublisher.close();
     }
 }

photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java

@@ -19,7 +19,6 @@ package org.photonvision.targeting;
 
 import java.util.ArrayList;
 import java.util.List;
-import java.util.Objects;
 import org.photonvision.common.dataflow.structures.Packet;
 
 /** Represents a pipeline result from a PhotonCamera. */
@@ -123,21 +122,6 @@ public class PhotonPipelineResult {
         return new ArrayList<>(targets);
     }
 
-    @Override
-    public boolean equals(Object o) {
-        if (this == o) return true;
-        if (o == null || getClass() != o.getClass()) return false;
-        PhotonPipelineResult that = (PhotonPipelineResult) o;
-        boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0;
-        boolean targetsMatch = that.targets.equals(targets);
-        return latencyMatch && targetsMatch;
-    }
-
-    @Override
-    public int hashCode() {
-        return Objects.hash(latencyMillis, targets);
-    }
-
     /**
      * Populates the fields of the pipeline result from the packet.
      *
@@ -178,4 +162,33 @@ public class PhotonPipelineResult {
         // Return the packet.
         return packet;
     }
+
+    @Override
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        result = prime * result + ((targets == null) ? 0 : targets.hashCode());
+        long temp;
+        temp = Double.doubleToLongBits(latencyMillis);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(timestampSeconds);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj == null) return false;
+        if (getClass() != obj.getClass()) return false;
+        PhotonPipelineResult other = (PhotonPipelineResult) obj;
+        if (targets == null) {
+            if (other.targets != null) return false;
+        } else if (!targets.equals(other.targets)) return false;
+        if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis))
+            return false;
+        if (Double.doubleToLongBits(timestampSeconds)
+                != Double.doubleToLongBits(other.timestampSeconds)) return false;
+        return true;
+    }
 }

photonlib-cpp-examples/aimandrange/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.2.1"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

photonlib-cpp-examples/aimattarget/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.2.1"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

photonlib-cpp-examples/apriltagExample/.gitignore

@@ -0,0 +1 @@
+vendordeps

photonlib-cpp-examples/apriltagExample/.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": true,
+    "currentLanguage": "cpp",
+    "projectYear": "2023",
+    "teamNumber": 5
+}

photonlib-cpp-examples/apriltagExample/WPILib-License.md

@@ -0,0 +1,24 @@
+Copyright (c) 2009-2021 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

photonlib-cpp-examples/apriltagExample/build.gradle

@@ -0,0 +1,116 @@
+plugins {
+    id "cpp"
+    id "google-test-test-suite"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
+
+    id "com.dorongold.task-tree" version "2.1.0"
+}
+
+repositories {
+    mavenLocal()
+    jcenter()
+}
+
+apply from: "${rootDir}/../shared/examples_common.gradle"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+    targets {
+        roborio(getTargetTypeClass('RoboRIO')) {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = project.frc.getTeamOrDefault(5940)
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcCpp(getArtifactTypeClass('FRCNativeArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Set to true to run simulation in debug mode
+wpi.cpp.debugSimulation = false
+
+// Default enable simgui
+wpi.sim.addGui().defaultEnabled = true
+// Enable DS but not by default
+wpi.sim.addDriverstation()
+
+model {
+    components {
+        frcUserProgram(NativeExecutableSpec) {
+            // We don't need to build for roborio -- if we do, we need to install
+            // a roborio toolchain every time we build in CI
+            // Ideally, we'd be able to set the roborio toolchain as optional, but
+            // I can't figure out how to set that environment variable from build.gradle
+            // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71)
+            // for now, commented out
+
+            // targetPlatform wpi.platforms.roborio
+
+            if (includeDesktopSupport) {
+                targetPlatform wpi.platforms.desktop
+            }
+
+            sources.cpp {
+                source {
+                    srcDir 'src/main/cpp'
+                    include '**/*.cpp', '**/*.cc'
+                }
+                exportedHeaders {
+                    srcDir 'src/main/include'
+                }
+            }
+
+            // Set deploy task to deploy this component
+            deployArtifact.component = it
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            // Enable simulation for this component
+            wpi.sim.enable(it)
+            // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+        }
+    }
+    testSuites {
+        frcUserProgramTest(GoogleTestTestSuiteSpec) {
+            testing $.components.frcUserProgram
+
+            sources.cpp {
+                source {
+                    srcDir 'src/test/cpp'
+                    include '**/*.cpp'
+                }
+            }
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+            wpi.cpp.deps.googleTest(it)
+        }
+    }
+}

photonlib-cpp-examples/apriltagExample/gradlew

@@ -0,0 +1,240 @@
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 the original authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+##############################################################################
+#
+#   Gradle start up script for POSIX generated by Gradle.
+#
+#   Important for running:
+#
+#   (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+#       noncompliant, but you have some other compliant shell such as ksh or
+#       bash, then to run this script, type that shell name before the whole
+#       command line, like:
+#
+#           ksh Gradle
+#
+#       Busybox and similar reduced shells will NOT work, because this script
+#       requires all of these POSIX shell features:
+#         * functions;
+#         * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+#           «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+#         * compound commands having a testable exit status, especially «case»;
+#         * various built-in commands including «command», «set», and «ulimit».
+#
+#   Important for patching:
+#
+#   (2) This script targets any POSIX shell, so it avoids extensions provided
+#       by Bash, Ksh, etc; in particular arrays are avoided.
+#
+#       The "traditional" practice of packing multiple parameters into a
+#       space-separated string is a well documented source of bugs and security
+#       problems, so this is (mostly) avoided, by progressively accumulating
+#       options in "$@", and eventually passing that to Java.
+#
+#       Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+#       and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+#       see the in-line comments for details.
+#
+#       There are tweaks for specific operating systems such as AIX, CygWin,
+#       Darwin, MinGW, and NonStop.
+#
+#   (3) This script is generated from the Groovy template
+#       https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+    APP_HOME=${app_path%"${app_path##*/}"}  # leaves a trailing /; empty if no leading path
+    [ -h "$app_path" ]
+do
+    ls=$( ls -ld "$app_path" )
+    link=${ls#*' -> '}
+    case $link in             #(
+      /*)   app_path=$link ;; #(
+      *)    app_path=$APP_HOME$link ;;
+    esac
+done
+
+APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
+
+APP_NAME="Gradle"
+APP_BASE_NAME=${0##*/}
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+    echo "$*"
+} >&2
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in                #(
+  CYGWIN* )         cygwin=true  ;; #(
+  Darwin* )         darwin=true  ;; #(
+  MSYS* | MINGW* )  msys=true    ;; #(
+  NONSTOP* )        nonstop=true ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD=$JAVA_HOME/jre/sh/java
+    else
+        JAVACMD=$JAVA_HOME/bin/java
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD=java
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+#   * args from the command line
+#   * the main class name
+#   * -classpath
+#   * -D...appname settings
+#   * --module-path (only if needed)
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if "$cygwin" || "$msys" ; then
+    APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+    JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    for arg do
+        if
+            case $arg in                                #(
+              -*)   false ;;                            # don't mess with options #(
+              /?*)  t=${arg#/} t=/${t%%/*}              # looks like a POSIX filepath
+                    [ -e "$t" ] ;;                      #(
+              *)    false ;;
+            esac
+        then
+            arg=$( cygpath --path --ignore --mixed "$arg" )
+        fi
+        # Roll the args list around exactly as many times as the number of
+        # args, so each arg winds up back in the position where it started, but
+        # possibly modified.
+        #
+        # NB: a `for` loop captures its iteration list before it begins, so
+        # changing the positional parameters here affects neither the number of
+        # iterations, nor the values presented in `arg`.
+        shift                   # remove old arg
+        set -- "$@" "$arg"      # push replacement arg
+    done
+fi
+
+# Collect all arguments for the java command;
+#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+#     shell script including quotes and variable substitutions, so put them in
+#     double quotes to make sure that they get re-expanded; and
+#   * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
+
+exec "$JAVACMD" "$@"

photonlib-cpp-examples/apriltagExample/gradlew.bat

@@ -0,0 +1,91 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega

photonlib-cpp-examples/apriltagExample/networktables.json

@@ -0,0 +1 @@
+[]

photonlib-cpp-examples/apriltagExample/settings.gradle

@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+rootProject.name = 'aimattarget'
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        jcenter()
+        gradlePluginPortal()
+        String frcYear = '2023'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name 'frcHome'
+            url frcHomeMaven
+        }
+    }
+}

photonlib-cpp-examples/apriltagExample/simgui-ds.json

@@ -0,0 +1,102 @@
+{
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {},
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decayRate": 0.0,
+          "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 65,
+          "incKey": 68
+        }
+      ],
+      "axisCount": 6,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

photonlib-cpp-examples/apriltagExample/simgui-window.json

@@ -0,0 +1,81 @@
+{
+  "Docking": {
+    "Data": []
+  },
+  "MainWindow": {
+    "GLOBAL": {
+      "fps": "120",
+      "height": "880",
+      "maximized": "0",
+      "style": "0",
+      "userScale": "2",
+      "width": "1652",
+      "xpos": "268",
+      "ypos": "82"
+    }
+  },
+  "Table": {
+    "0x542B5671,2": {
+      "Column 0  Width": "391",
+      "Column 1  Width": "156",
+      "RefScale": "13"
+    }
+  },
+  "Window": {
+    "###/SmartDashboard/Field": {
+      "Collapsed": "0",
+      "Pos": "514,2",
+      "Size": "517,341"
+    },
+    "###FMS": {
+      "Collapsed": "0",
+      "Pos": "36,663",
+      "Size": "283,146"
+    },
+    "###Joysticks": {
+      "Collapsed": "0",
+      "Pos": "373,500",
+      "Size": "796,206"
+    },
+    "###Keyboard 0 Settings": {
+      "Collapsed": "0",
+      "Pos": "149,98",
+      "Size": "300,560"
+    },
+    "###NetworkTables": {
+      "Collapsed": "0",
+      "Pos": "663,464",
+      "Size": "750,365"
+    },
+    "###NetworkTables Info": {
+      "Collapsed": "0",
+      "Pos": "520,330",
+      "Size": "750,145"
+    },
+    "###Other Devices": {
+      "Collapsed": "0",
+      "Pos": "1025,20",
+      "Size": "250,695"
+    },
+    "###System Joysticks": {
+      "Collapsed": "0",
+      "Pos": "5,350",
+      "Size": "192,218"
+    },
+    "###Timing": {
+      "Collapsed": "0",
+      "Pos": "5,150",
+      "Size": "135,127"
+    },
+    "Debug##Default": {
+      "Collapsed": "0",
+      "Pos": "60,60",
+      "Size": "400,400"
+    },
+    "Robot State": {
+      "Collapsed": "0",
+      "Pos": "5,20",
+      "Size": "92,99"
+    }
+  }
+}

photonlib-cpp-examples/apriltagExample/simgui.json

@@ -0,0 +1,40 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Field": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "CameraPublisher": {
+        "open": true
+      },
+      "SmartDashboard": {
+        "open": true
+      },
+      "photonvision": {
+        "open": true,
+        "photonvision": {
+          "open": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "Server": {
+      "open": true
+    },
+    "visible": true
+  }
+}

photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp

@@ -0,0 +1,92 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "Drivetrain.h"
+
+#include <frc/RobotController.h>
+#include <frc/smartdashboard/Field2d.h>
+
+void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
+  auto leftFeedforward = m_feedforward.Calculate(speeds.left);
+  auto rightFeedforward = m_feedforward.Calculate(speeds.right);
+  double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(),
+                                                    speeds.left.value());
+  double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(),
+                                                      speeds.right.value());
+
+  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::radians_per_second_t rot) {
+  SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_poseEstimator.Update(m_gyro.GetRotation2d(),
+                         units::meter_t{m_leftEncoder.GetDistance()},
+                         units::meter_t{m_rightEncoder.GetDistance()});
+}
+
+void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+  m_drivetrainSimulator.SetPose(pose);
+  m_poseEstimator.ResetPosition(
+      m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
+      units::meter_t{m_rightEncoder.GetDistance()}, pose);
+}
+
+void Drivetrain::SimulationPeriodic() {
+  // To update our simulation, we set motor voltage inputs, update the
+  // simulation, and write the simulated positions and velocities to our
+  // simulated encoder and gyro. We negate the right side so that positive
+  // voltages make the right side move forward.
+
+  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftGroup.Get()} *
+                                      frc::RobotController::GetInputVoltage(),
+                                  units::volt_t{m_rightGroup.Get()} *
+                                      frc::RobotController::GetInputVoltage());
+  m_drivetrainSimulator.Update(20_ms);
+
+  m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
+  m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
+  m_rightEncoderSim.SetDistance(
+      m_drivetrainSimulator.GetRightPosition().value());
+  m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
+  m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value());
+}
+
+void Drivetrain::Periodic() {
+  UpdateOdometry();
+
+  auto result = m_pcw.Update(m_poseEstimator.GetEstimatedPosition());
+  if (result) {
+    m_poseEstimator.AddVisionMeasurement(
+        result.value().estimatedPose.ToPose2d(), result.value().timestamp);
+  }
+
+  m_fieldSim.SetRobotPose(m_poseEstimator.GetEstimatedPosition());
+}

photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp

@@ -0,0 +1,61 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "Robot.h"
+
+#include <frc/RobotBase.h>
+#include <networktables/NetworkTable.h>
+
+void Robot::RobotInit() {
+  if constexpr (frc::RobotBase::IsSimulation()) {
+    auto inst = nt::NetworkTableInstance::GetDefault();
+    inst.StopServer();
+    // set the NT server if simulating this code.
+    // "localhost" for photon on desktop, or "photonvision.local" or
+    // "[ip-address]" for coprocessor
+    inst.SetServer("localhost");
+    inst.StartClient4("Robot Simulation");
+  }
+}
+
+void Robot::TeleopPeriodic() {
+  // Get the x speed. We are inverting this because Xbox controllers return
+  // negative values when we push forward.
+  const auto xSpeed = -m_controller.GetLeftY() * Drivetrain::kMaxSpeed;
+
+  // Get the rate of angular rotation. We are inverting this because we want a
+  // positive value when we pull to the left (remember, CCW is positive in
+  // mathematics). Xbox controllers return positive values when you pull to
+  // the right by default.
+  auto rot = -m_controller.GetRightX() * Drivetrain::kMaxAngularSpeed;
+
+  m_drive.Drive(xSpeed, rot);
+}
+
+void Robot::RobotPeriodic() { m_drive.Periodic(); }
+void Robot::SimulationPeriodic() { m_drive.SimulationPeriodic(); }
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h

@@ -0,0 +1,140 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <numbers>
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/estimator/DifferentialDrivePoseEstimator.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/AnalogGyroSim.h>
+#include <frc/simulation/DifferentialDrivetrainSim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/smartdashboard/Field2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+
+#include "PhotonCameraWrapper.h"
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() {
+    m_gyro.Reset();
+
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.SetInverted(true);
+
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
+                                      kEncoderResolution);
+    m_rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
+                                       kEncoderResolution);
+
+    m_leftEncoder.Reset();
+    m_rightEncoder.Reset();
+
+    m_rightGroup.SetInverted(true);
+
+    frc::SmartDashboard::PutData("Field", &m_fieldSim);
+  }
+
+  static constexpr units::meters_per_second_t kMaxSpeed =
+      3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      std::numbers::pi};  // 1/2 rotation per second
+
+  void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
+  void Drive(units::meters_per_second_t xSpeed,
+             units::radians_per_second_t rot);
+  void UpdateOdometry();
+  void ResetOdometry(const frc::Pose2d& pose);
+
+  frc::Pose2d GetPose() const { return m_poseEstimator.GetEstimatedPosition(); }
+
+  void SimulationPeriodic();
+  void Periodic();
+
+ private:
+  static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
+  static constexpr double kWheelRadius = 0.0508;  // meters
+  static constexpr int kEncoderResolution = 4096;
+
+  frc::PWMSparkMax m_leftLeader{1};
+  frc::PWMSparkMax m_leftFollower{2};
+  frc::PWMSparkMax m_rightLeader{3};
+  frc::PWMSparkMax m_rightFollower{4};
+
+  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
+  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
+
+  frc::Encoder m_leftEncoder{0, 1};
+  frc::Encoder m_rightEncoder{2, 3};
+
+  frc2::PIDController m_leftPIDController{8.5, 0.0, 0.0};
+  frc2::PIDController m_rightPIDController{8.5, 0.0, 0.0};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
+
+  frc::DifferentialDrivePoseEstimator m_poseEstimator{
+      m_kinematics, m_gyro.GetRotation2d(),
+      units::meter_t{m_leftEncoder.GetDistance()},
+      units::meter_t{m_rightEncoder.GetDistance()}, frc::Pose2d{}};
+
+  PhotonCameraWrapper m_pcw;
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
+
+  // Simulation classes help us simulate our robot
+  frc::sim::AnalogGyroSim m_gyroSim{m_gyro};
+  frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
+  frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
+  frc::Field2d m_fieldSim;
+  frc::LinearSystem<2, 2, 2> m_drivetrainSystem =
+      frc::LinearSystemId::IdentifyDrivetrainSystem(
+          1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
+  frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
+      m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in};
+};

photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h

@@ -0,0 +1,47 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <photonlib/PhotonCamera.h>
+#include <photonlib/PhotonPoseEstimator.h>
+
+#include <utility>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+
+class PhotonCameraWrapper {
+ public:
+  photonlib::PhotonPoseEstimator m_poseEstimator{
+      frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
+      photonlib::MULTI_TAG_PNP, std::move(photonlib::PhotonCamera{"WPI2023"}),
+      frc::Transform3d{}};
+
+  inline std::optional<photonlib::EstimatedRobotPose> Update(
+      frc::Pose2d estimatedPose) {
+    m_poseEstimator.SetReferencePose(frc::Pose3d(estimatedPose));
+    return m_poseEstimator.Update();
+  }
+};

photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h

@@ -0,0 +1,50 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <photonlib/PhotonCamera.h>
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMVictorSPX.h>
+#include <units/angle.h>
+#include <units/length.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void TeleopPeriodic() override;
+  void SimulationPeriodic() override;
+
+ private:
+  frc::XboxController m_controller{0};
+
+  Drivetrain m_drive;
+};

photonlib-cpp-examples/apriltagExample/src/test/cpp/main.cpp

@@ -0,0 +1,34 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

photonlib-cpp-examples/examples.txt

@@ -1,3 +1,4 @@
 aimandrange
 getinrange
 aimattarget
+apriltagExample

photonlib-cpp-examples/getinrange/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.2.1"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

photonlib-java-examples/aimandrange/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.2.1"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/aimattarget/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.2.1"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/apriltagExample/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.2.1"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java

@@ -181,9 +181,6 @@ public class Drivetrain {
         m_poseEstimator.update(
                 m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
 
-        // Also apply vision measurements. We use 0.3 seconds in the past as an example
-        // -- on
-        // a real robot, this must be calculated based either on latency or timestamps.
         Optional<EstimatedRobotPose> result =
                 pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition());
 

photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java

@@ -24,14 +24,12 @@
 
 package frc.robot;
 
-import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import frc.robot.Constants.FieldConstants;
+import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.Constants.VisionConstants;
-import java.util.ArrayList;
+import java.io.IOException;
 import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
@@ -39,50 +37,39 @@ import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 
 public class PhotonCameraWrapper {
-    public PhotonCamera photonCamera;
-    public PhotonPoseEstimator photonPoseEstimator;
+    private PhotonCamera photonCamera;
+    private PhotonPoseEstimator photonPoseEstimator;
 
     public PhotonCameraWrapper() {
-        // Set up a test arena of two apriltags at the center of each driver station set
-        final AprilTag tag18 =
-                new AprilTag(
-                        18,
-                        new Pose3d(
-                                new Pose2d(
-                                        FieldConstants.length,
-                                        FieldConstants.width / 2.0,
-                                        Rotation2d.fromDegrees(180))));
-        final AprilTag tag01 =
-                new AprilTag(
-                        01,
-                        new Pose3d(new Pose2d(0.0, FieldConstants.width / 2.0, Rotation2d.fromDegrees(0.0))));
-        ArrayList<AprilTag> atList = new ArrayList<AprilTag>();
-        atList.add(tag18);
-        atList.add(tag01);
+        // Change the name of your camera here to whatever it is in the PhotonVision UI.
+        photonCamera = new PhotonCamera(VisionConstants.cameraName);
 
-        // TODO - once 2023 happens, replace this with just loading the 2023 field arrangement
-        AprilTagFieldLayout atfl =
-                new AprilTagFieldLayout(atList, FieldConstants.length, FieldConstants.width);
-
-        // Forward Camera
-        photonCamera =
-                new PhotonCamera(
-                        VisionConstants
-                                .cameraName); // Change the name of your camera here to whatever it is in the
-        // PhotonVision UI.
-
-        // Create pose estimator
-        photonPoseEstimator =
-                new PhotonPoseEstimator(
-                        atfl, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, photonCamera, VisionConstants.robotToCam);
+        try {
+            // Attempt to load the AprilTagFieldLayout that will tell us where the tags are on the field.
+            AprilTagFieldLayout fieldLayout = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField();
+            // Create pose estimator
+            photonPoseEstimator =
+                    new PhotonPoseEstimator(
+                            fieldLayout, PoseStrategy.MULTI_TAG_PNP, photonCamera, VisionConstants.robotToCam);
+            photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+        } catch (IOException e) {
+            // The AprilTagFieldLayout failed to load. We won't be able to estimate poses if we don't know
+            // where the tags are.
+            DriverStation.reportError("Failed to load AprilTagFieldLayout", e.getStackTrace());
+            photonPoseEstimator = null;
+        }
     }
 
     /**
      * @param estimatedRobotPose The current best guess at robot pose
-     * @return A pair of the fused camera observations to a single Pose2d on the field, and the time
-     *     of the observation. Assumes a planar field and the robot is always firmly on the ground
+     * @return an EstimatedRobotPose with an estimated pose, the timestamp, and targets used to create
+     *     the estimate
      */
     public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
+        if (photonPoseEstimator == null) {
+            // The field layout failed to load, so we cannot estimate poses.
+            return Optional.empty();
+        }
         photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
         return photonPoseEstimator.update();
     }

photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java

@@ -46,7 +46,7 @@ public class Robot extends TimedRobot {
             // set the NT server if simulating this code.
             // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]" for coprocessor
             instance.setServer("localhost");
-            instance.startClient4("myRobot");
+            instance.startClient4("Robot Simulation");
         }
 
         m_controller = new XboxController(0);

photonlib-java-examples/getinrange/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.2.1"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/simaimandrange/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.2.1"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/simposeest/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.2.1"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

shared/config.gradle

@@ -8,9 +8,9 @@ nativeUtils.withCrossLinuxArm64()
 nativeUtils.wpi.configureDependencies {
     wpiVersion = wpilibVersion
     wpimathVersion = wpilibVersion
-    niLibVersion = "2023.2.0"
-    opencvVersion = "4.6.0-3"
-    googleTestVersion = "1.11.0-4"
+    niLibVersion = "2023.3.0"
+    opencvVersion = "4.6.0-4"
+    googleTestVersion = "1.12.1-1"
     imguiVersion = "1.86-1"
 }