[examples] Add example programs for AprilTags detection (#4932)

Co-authored-by: Peter Johnson <johnson.peter@gmail.com>

.clang-format

@@ -1,5 +1,4 @@
 ---
-Language:        Cpp
 BasedOnStyle:  Google
 AccessModifierOffset: -1
 AlignAfterOpenBracket: Align

.github/workflows/gradle.yml

@@ -46,7 +46,7 @@ jobs:
 
   build-host:
     env:
-      MACOSX_DEPLOYMENT_TARGET: 10.14
+      MACOSX_DEPLOYMENT_TARGET: 10.15
     strategy:
       fail-fast: false
       matrix:

.gitignore

@@ -19,6 +19,9 @@ test-reports
 .idea/
 out/
 
+# Fleet
+.fleet
+
 # Created by http://www.gitignore.io
 
 ### Linux ###

.styleguide

@@ -18,6 +18,7 @@ generatedFileExclude {
   FRCNetComm\.java$
   simulation/gz_msgs/src/include/simulation/gz_msgs/msgs\.h$
   fieldImages/src/main/native/resources/
+  apriltag/src/test/resources/
 }
 
 repoRootNameOverride {

apriltag/CMakeLists.txt

@@ -40,7 +40,7 @@ if (WITH_JAVA)
   set(CMAKE_JAVA_INCLUDE_PATH apriltag.jar ${EJML_JARS} ${JACKSON_JARS})
 
   file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-  file(GLOB_RECURSE JAVA_RESOURCES src/main/native/resources/*.json)
+  file(GLOB_RECURSE JAVA_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} src/main/native/resources/*.json)
   add_jar(apriltag_jar
     SOURCES ${JAVA_SOURCES}
     RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES}

apriltag/build.gradle

@@ -4,6 +4,11 @@ ext {
     nativeName = 'apriltag'
     devMain = 'edu.wpi.first.apriltag.DevMain'
     useJava = true
+    useCpp = true
+    sharedCvConfigs = [
+        apriltagDev : [],
+        apriltagTest: []]
+    staticCvConfigs = []
 
     def generateTask = createGenerateResourcesTask('main', 'APRILTAG', 'frc', project)
 
@@ -30,6 +35,7 @@ apply from: "${rootDir}/shared/opencv.gradle"
 
 dependencies {
     implementation project(':wpimath')
+    devImplementation project(':wpimath')
 }
 
 sourceSets {
@@ -40,7 +46,6 @@ sourceSets {
     }
 }
 
-
 model {
     components {}
     binaries {

apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java

@@ -8,6 +8,12 @@ public final class DevMain {
   /** Main entry point. */
   public static void main(String[] args) {
     System.out.println("Hello World!");
+    AprilTagDetector detector = new AprilTagDetector();
+    detector.addFamily("tag16h5");
+    AprilTagDetector.Config config = new AprilTagDetector.Config();
+    config.refineEdges = false;
+    detector.setConfig(config);
+    detector.close();
   }
 
   private DevMain() {}

apriltag/src/dev/native/cpp/main.cpp

@@ -2,4 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-int main() {}
+#include "frc/apriltag/AprilTagDetector.h"
+
+int main() {
+  frc::AprilTagDetector detector;
+  detector.AddFamily("tag16h5");
+  detector.SetConfig({.refineEdges = false});
+}

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java

@@ -0,0 +1,190 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.apriltag;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Arrays;
+
+/** A detection of an AprilTag tag. */
+public class AprilTagDetection {
+  /**
+   * Gets the decoded tag's family name.
+   *
+   * @return Decoded family name
+   */
+  public String getFamily() {
+    return m_family;
+  }
+
+  /**
+   * Gets the decoded ID of the tag.
+   *
+   * @return Decoded ID
+   */
+  public int getId() {
+    return m_id;
+  }
+
+  /**
+   * Gets how many error bits were corrected. Note: accepting large numbers of corrected errors
+   * leads to greatly increased false positive rates. NOTE: As of this implementation, the detector
+   * cannot detect tags with a hamming distance greater than 2.
+   *
+   * @return Hamming distance (number of corrected error bits)
+   */
+  public int getHamming() {
+    return m_hamming;
+  }
+
+  /**
+   * Gets a measure of the quality of the binary decoding process: the average difference between
+   * the intensity of a data bit versus the decision threshold. Higher numbers roughly indicate
+   * better decodes. This is a reasonable measure of detection accuracy only for very small tags--
+   * not effective for larger tags (where we could have sampled anywhere within a bit cell and still
+   * gotten a good detection.)
+   *
+   * @return Decision margin
+   */
+  public float getDecisionMargin() {
+    return m_decisionMargin;
+  }
+
+  /**
+   * Gets the 3x3 homography matrix describing the projection from an "ideal" tag (with corners at
+   * (-1,1), (1,1), (1,-1), and (-1, -1)) to pixels in the image.
+   *
+   * @return Homography matrix data
+   */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
+  public double[] getHomography() {
+    return m_homography;
+  }
+
+  /**
+   * Gets the 3x3 homography matrix describing the projection from an "ideal" tag (with corners at
+   * (-1,1), (1,1), (1,-1), and (-1, -1)) to pixels in the image.
+   *
+   * @return Homography matrix
+   */
+  public Matrix<N3, N3> getHomographyMatrix() {
+    return new MatBuilder<>(Nat.N3(), Nat.N3()).fill(m_homography);
+  }
+
+  /**
+   * Gets the center of the detection in image pixel coordinates.
+   *
+   * @return Center point X coordinate
+   */
+  public double getCenterX() {
+    return m_centerX;
+  }
+
+  /**
+   * Gets the center of the detection in image pixel coordinates.
+   *
+   * @return Center point Y coordinate
+   */
+  public double getCenterY() {
+    return m_centerY;
+  }
+
+  /**
+   * Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise
+   * around the tag.
+   *
+   * @param ndx Corner index (range is 0-3, inclusive)
+   * @return Corner point X coordinate
+   */
+  public double getCornerX(int ndx) {
+    return m_corners[ndx * 2];
+  }
+
+  /**
+   * Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise
+   * around the tag.
+   *
+   * @param ndx Corner index (range is 0-3, inclusive)
+   * @return Corner point Y coordinate
+   */
+  public double getCornerY(int ndx) {
+    return m_corners[ndx * 2 + 1];
+  }
+
+  /**
+   * Gets the corners of the tag in image pixel coordinates. These always wrap counter-clock wise
+   * around the tag.
+   *
+   * @return Corner point array (X and Y for each corner in order)
+   */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
+  public double[] getCorners() {
+    return m_corners;
+  }
+
+  private final String m_family;
+  private final int m_id;
+  private final int m_hamming;
+  private final float m_decisionMargin;
+  private final double[] m_homography;
+  private final double m_centerX;
+  private final double m_centerY;
+  private final double[] m_corners;
+
+  /**
+   * Constructs a new detection result. Used from JNI.
+   *
+   * @param family family
+   * @param id id
+   * @param hamming hamming
+   * @param decisionMargin dm
+   * @param homography homography
+   * @param centerX centerX
+   * @param centerY centerY
+   * @param corners corners
+   */
+  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+  public AprilTagDetection(
+      String family,
+      int id,
+      int hamming,
+      float decisionMargin,
+      double[] homography,
+      double centerX,
+      double centerY,
+      double[] corners) {
+    m_family = family;
+    m_id = id;
+    m_hamming = hamming;
+    m_decisionMargin = decisionMargin;
+    m_homography = homography;
+    m_centerX = centerX;
+    m_centerY = centerY;
+    m_corners = corners;
+  }
+
+  @Override
+  public String toString() {
+    return "DetectionResult [centerX="
+        + m_centerX
+        + ", centerY="
+        + m_centerY
+        + ", corners="
+        + Arrays.toString(m_corners)
+        + ", decisionMargin="
+        + m_decisionMargin
+        + ", hamming="
+        + m_hamming
+        + ", homography="
+        + Arrays.toString(m_homography)
+        + ", family="
+        + m_family
+        + ", id="
+        + m_id
+        + "]";
+  }
+}

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java

@@ -0,0 +1,281 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.apriltag;
+
+import edu.wpi.first.apriltag.jni.AprilTagJNI;
+import org.opencv.core.Mat;
+
+/**
+ * An AprilTag detector engine. This is expensive to set up and tear down, so most use cases should
+ * only create one of these, add a family to it, set up any other configuration, and repeatedly call
+ * Detect().
+ */
+public class AprilTagDetector implements AutoCloseable {
+  /** Detector configuration. */
+  @SuppressWarnings("MemberName")
+  public static class Config {
+    /**
+     * How many threads should be used for computation. Default is single-threaded operation (1
+     * thread).
+     */
+    public int numThreads = 1;
+
+    /**
+     * Quad decimation. Detection of quads can be done on a lower-resolution image, improving speed
+     * at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary
+     * payload is still done at full resolution. Default is 2.0.
+     */
+    public float quadDecimate = 2.0f;
+
+    /**
+     * What Gaussian blur should be applied to the segmented image (used for quad detection). Very
+     * noisy images benefit from non-zero values (e.g. 0.8). Default is 0.0.
+     */
+    public float quadSigma;
+
+    /**
+     * When true, the edges of the each quad are adjusted to "snap to" strong gradients nearby. This
+     * is useful when decimation is employed, as it can increase the quality of the initial quad
+     * estimate substantially. Generally recommended to be on (true). Default is true.
+     *
+     * <p>Very computationally inexpensive. Option is ignored if quad_decimate = 1.
+     */
+    public boolean refineEdges = true;
+
+    /**
+     * How much sharpening should be done to decoded images. This can help decode small tags but may
+     * or may not help in odd lighting conditions or low light conditions. Default is 0.25.
+     */
+    public double decodeSharpening = 0.25;
+
+    /**
+     * Debug mode. When true, the decoder writes a variety of debugging images to the current
+     * working directory at various stages through the detection process. This is slow and should
+     * *not* be used on space-limited systems such as the RoboRIO. Default is disabled (false).
+     */
+    public boolean debug;
+
+    public Config() {}
+
+    Config(
+        int numThreads,
+        float quadDecimate,
+        float quadSigma,
+        boolean refineEdges,
+        double decodeSharpening,
+        boolean debug) {
+      this.numThreads = numThreads;
+      this.quadDecimate = quadDecimate;
+      this.quadSigma = quadSigma;
+      this.refineEdges = refineEdges;
+      this.decodeSharpening = decodeSharpening;
+      this.debug = debug;
+    }
+
+    @Override
+    public int hashCode() {
+      return numThreads
+          + Float.hashCode(quadDecimate)
+          + Float.hashCode(quadSigma)
+          + Boolean.hashCode(refineEdges)
+          + Double.hashCode(decodeSharpening)
+          + Boolean.hashCode(debug);
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (!(obj instanceof Config)) {
+        return false;
+      }
+
+      Config other = (Config) obj;
+      return numThreads == other.numThreads
+          && quadDecimate == other.quadDecimate
+          && quadSigma == other.quadSigma
+          && refineEdges == other.refineEdges
+          && decodeSharpening == other.decodeSharpening
+          && debug == other.debug;
+    }
+  }
+
+  /** Quad threshold parameters. */
+  @SuppressWarnings("MemberName")
+  public static class QuadThresholdParameters {
+    /** Threshold used to reject quads containing too few pixels. Default is 5 pixels. */
+    public int minClusterPixels = 5;
+
+    /**
+     * How many corner candidates to consider when segmenting a group of pixels into a quad. Default
+     * is 10.
+     */
+    public int maxNumMaxima = 10;
+
+    /**
+     * Critical angle, in radians. The detector will reject quads where pairs of edges have angles
+     * that are close to straight or close to 180 degrees. Zero means that no quads are rejected.
+     * Default is 10 degrees.
+     */
+    public double criticalAngle = 10 * Math.PI / 180.0;
+
+    /**
+     * When fitting lines to the contours, the maximum mean squared error allowed. This is useful in
+     * rejecting contours that are far from being quad shaped; rejecting these quads "early" saves
+     * expensive decoding processing. Default is 10.0.
+     */
+    public float maxLineFitMSE = 10.0f;
+
+    /**
+     * Minimum brightness offset. When we build our model of black &amp; white pixels, we add an
+     * extra check that the white model must be (overall) brighter than the black model. How much
+     * brighter? (in pixel values, [0,255]). Default is 5.
+     */
+    public int minWhiteBlackDiff = 5;
+
+    /**
+     * Whether the thresholded image be should be deglitched. Only useful for very noisy images.
+     * Default is disabled (false).
+     */
+    public boolean deglitch;
+
+    public QuadThresholdParameters() {}
+
+    QuadThresholdParameters(
+        int minClusterPixels,
+        int maxNumMaxima,
+        double criticalAngle,
+        float maxLineFitMSE,
+        int minWhiteBlackDiff,
+        boolean deglitch) {
+      this.minClusterPixels = minClusterPixels;
+      this.maxNumMaxima = maxNumMaxima;
+      this.criticalAngle = criticalAngle;
+      this.maxLineFitMSE = maxLineFitMSE;
+      this.minWhiteBlackDiff = minWhiteBlackDiff;
+      this.deglitch = deglitch;
+    }
+
+    @Override
+    public int hashCode() {
+      return minClusterPixels
+          + maxNumMaxima
+          + Double.hashCode(criticalAngle)
+          + Float.hashCode(maxLineFitMSE)
+          + minWhiteBlackDiff
+          + Boolean.hashCode(deglitch);
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (!(obj instanceof QuadThresholdParameters)) {
+        return false;
+      }
+
+      QuadThresholdParameters other = (QuadThresholdParameters) obj;
+      return minClusterPixels == other.minClusterPixels
+          && maxNumMaxima == other.maxNumMaxima
+          && criticalAngle == other.criticalAngle
+          && maxLineFitMSE == other.maxLineFitMSE
+          && minWhiteBlackDiff == other.minWhiteBlackDiff
+          && deglitch == other.deglitch;
+    }
+  }
+
+  public AprilTagDetector() {
+    m_native = AprilTagJNI.createDetector();
+  }
+
+  @Override
+  public void close() {
+    if (m_native != 0) {
+      AprilTagJNI.destroyDetector(m_native);
+    }
+    m_native = 0;
+  }
+
+  /**
+   * Sets detector configuration.
+   *
+   * @param config Configuration
+   */
+  public void setConfig(Config config) {
+    AprilTagJNI.setDetectorConfig(m_native, config);
+  }
+
+  /**
+   * Gets detector configuration.
+   *
+   * @return Configuration
+   */
+  public Config getConfig() {
+    return AprilTagJNI.getDetectorConfig(m_native);
+  }
+
+  /**
+   * Sets quad threshold parameters.
+   *
+   * @param params Parameters
+   */
+  public void setQuadThresholdParameters(QuadThresholdParameters params) {
+    AprilTagJNI.setDetectorQTP(m_native, params);
+  }
+
+  /**
+   * Gets quad threshold parameters.
+   *
+   * @return Parameters
+   */
+  public QuadThresholdParameters getQuadThresholdParameters() {
+    return AprilTagJNI.getDetectorQTP(m_native);
+  }
+
+  /**
+   * Adds a family of tags to be detected.
+   *
+   * @param fam Family name, e.g. "tag16h5"
+   * @throws IllegalArgumentException if family name not recognized
+   */
+  public void addFamily(String fam) {
+    addFamily(fam, 2);
+  }
+
+  /**
+   * Adds a family of tags to be detected.
+   *
+   * @param fam Family name, e.g. "tag16h5"
+   * @param bitsCorrected maximum number of bits to correct
+   * @throws IllegalArgumentException if family name not recognized
+   */
+  public void addFamily(String fam, int bitsCorrected) {
+    if (!AprilTagJNI.addFamily(m_native, fam, bitsCorrected)) {
+      throw new IllegalArgumentException("unknown family name '" + fam + "'");
+    }
+  }
+
+  /**
+   * Removes a family of tags from the detector.
+   *
+   * @param fam Family name, e.g. "tag16h5"
+   */
+  public void removeFamily(String fam) {
+    AprilTagJNI.removeFamily(m_native, fam);
+  }
+
+  /** Unregister all families. */
+  public void clearFamilies() {
+    AprilTagJNI.clearFamilies(m_native);
+  }
+
+  /**
+   * Detect tags from an 8-bit image.
+   *
+   * @param img 8-bit OpenCV Mat image
+   * @return Results (array of AprilTagDetection)
+   */
+  public AprilTagDetection[] detect(Mat img) {
+    return AprilTagJNI.detect(m_native, img.cols(), img.rows(), img.cols(), img.dataAddr());
+  }
+
+  private long m_native;
+}

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java

@@ -33,11 +33,13 @@ import java.util.Optional;
  * meters with "width" and "length" values. This is to account for arbitrary field sizes when
  * transforming the poses.
  *
- * <p>Pose3ds are assumed to be measured from the bottom-left corner of the field, when the blue
- * alliance is at the left. By default, Pose3ds will be returned as declared when calling {@link
- * AprilTagFieldLayout#getTagPose(int)}. {@link #setOrigin(OriginPosition)} can be used to transform
- * the poses returned from {@link AprilTagFieldLayout#getTagPose(int)} to be correct relative to a
- * different coordinate frame.
+ * <p>Pose3ds in the JSON are measured using the normal FRC coordinate system, NWU with the origin
+ * at the bottom-right corner of the blue alliance wall. {@link #setOrigin(OriginPosition)} can be
+ * used to change the poses returned from {@link AprilTagFieldLayout#getTagPose(int)} to be from the
+ * perspective of a specific alliance.
+ *
+ * <p>Tag poses represent the center of the tag, with a zero rotation representing a tag that is
+ * upright and facing away from the (blue) alliance wall (that is, towards the opposing alliance).
  */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
@@ -115,8 +117,8 @@ public class AprilTagFieldLayout {
    * Sets the origin based on a predefined enumeration of coordinate frame origins. The origins are
    * calculated from the field dimensions.
    *
-   * <p>This transforms the Pose3ds returned by {@link #getTagPose(int)} to return the correct pose
-   * relative to a predefined coordinate frame.
+   * <p>This transforms the Pose3d objects returned by {@link #getTagPose(int)} to return the
+   * correct pose relative to a predefined coordinate frame.
    *
    * @param origin The predefined origin
    */
@@ -140,8 +142,8 @@ public class AprilTagFieldLayout {
   /**
    * Sets the origin for tag pose transformation.
    *
-   * <p>This transforms the Pose3ds returned by {@link #getTagPose(int)} to return the correct pose
-   * relative to the provided origin.
+   * <p>This transforms the Pose3d objects returned by {@link #getTagPose(int)} to return the
+   * correct pose relative to the provided origin.
    *
    * @param origin The new origin for tag transformations
    */

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java

@@ -5,12 +5,13 @@
 package edu.wpi.first.apriltag;
 
 public enum AprilTagFields {
-  k2022RapidReact("2022-rapidreact.json");
+  k2022RapidReact("2022-rapidreact.json"),
+  k2023ChargedUp("2023-chargedup.json");
 
   public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
 
   /** Alias to the current game. */
-  public static final AprilTagFields kDefaultField = k2022RapidReact;
+  public static final AprilTagFields kDefaultField = k2023ChargedUp;
 
   public final String m_resourceFile;
 

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimate.java

@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.apriltag;
+
+import edu.wpi.first.math.geometry.Transform3d;
+
+/** A pair of AprilTag pose estimates. */
+@SuppressWarnings("MemberName")
+public class AprilTagPoseEstimate {
+  /**
+   * Constructs a pose estimate.
+   *
+   * @param pose1 first pose
+   * @param pose2 second pose
+   * @param error1 error of first pose
+   * @param error2 error of second pose
+   */
+  public AprilTagPoseEstimate(Transform3d pose1, Transform3d pose2, double error1, double error2) {
+    this.pose1 = pose1;
+    this.pose2 = pose2;
+    this.error1 = error1;
+    this.error2 = error2;
+  }
+
+  /**
+   * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
+   * ambiguous.
+   *
+   * @return The ratio of pose reprojection errors.
+   */
+  public double getAmbiguity() {
+    double min = Math.min(error1, error2);
+    double max = Math.max(error1, error2);
+
+    if (max > 0) {
+      return min / max;
+    } else {
+      return -1;
+    }
+  }
+
+  /** Pose 1. */
+  public final Transform3d pose1;
+
+  /** Pose 2. */
+  public final Transform3d pose2;
+
+  /** Object-space error of pose 1. */
+  public final double error1;
+
+  /** Object-space error of pose 2. */
+  public final double error2;
+}

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java

@@ -0,0 +1,190 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.apriltag;
+
+import edu.wpi.first.apriltag.jni.AprilTagJNI;
+import edu.wpi.first.math.geometry.Transform3d;
+
+/** Pose estimators for AprilTag tags. */
+public class AprilTagPoseEstimator {
+  /** Configuration for the pose estimator. */
+  @SuppressWarnings("MemberName")
+  public static class Config {
+    /**
+     * Creates a pose estimator configuration.
+     *
+     * @param tagSize tag size, in meters
+     * @param fx camera horizontal focal length, in pixels
+     * @param fy camera vertical focal length, in pixels
+     * @param cx camera horizontal focal center, in pixels
+     * @param cy camera vertical focal center, in pixels
+     */
+    public Config(double tagSize, double fx, double fy, double cx, double cy) {
+      this.tagSize = tagSize;
+      this.fx = fx;
+      this.fy = fy;
+      this.cx = cx;
+      this.cy = cy;
+    }
+
+    public double tagSize;
+    public double fx;
+    public double fy;
+    public double cx;
+    public double cy;
+
+    @Override
+    public int hashCode() {
+      return Double.hashCode(tagSize)
+          + Double.hashCode(fx)
+          + Double.hashCode(fy)
+          + Double.hashCode(cx)
+          + Double.hashCode(cy);
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (!(obj instanceof Config)) {
+        return false;
+      }
+
+      Config other = (Config) obj;
+      return tagSize == other.tagSize
+          && fx == other.fx
+          && fy == other.fy
+          && cx == other.cx
+          && cy == other.cy;
+    }
+  }
+
+  /**
+   * Creates estimator.
+   *
+   * @param config Configuration
+   */
+  public AprilTagPoseEstimator(Config config) {
+    m_config = new Config(config.tagSize, config.fx, config.fy, config.cx, config.cy);
+  }
+
+  /**
+   * Sets estimator configuration.
+   *
+   * @param config Configuration
+   */
+  public void setConfig(Config config) {
+    m_config.tagSize = config.tagSize;
+    m_config.fx = config.fx;
+    m_config.fy = config.fy;
+    m_config.cx = config.cx;
+    m_config.cy = config.cy;
+  }
+
+  /**
+   * Gets estimator configuration.
+   *
+   * @return Configuration
+   */
+  public Config getConfig() {
+    return new Config(m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy);
+  }
+
+  /**
+   * Estimates the pose of the tag using the homography method described in [1].
+   *
+   * @param detection Tag detection
+   * @return Pose estimate
+   */
+  public Transform3d estimateHomography(AprilTagDetection detection) {
+    return estimateHomography(detection.getHomography());
+  }
+
+  /**
+   * Estimates the pose of the tag using the homography method described in [1].
+   *
+   * @param homography Homography 3x3 matrix data
+   * @return Pose estimate
+   */
+  public Transform3d estimateHomography(double[] homography) {
+    return AprilTagJNI.estimatePoseHomography(
+        homography, m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy);
+  }
+
+  /**
+   * Estimates the pose of the tag. This returns one or two possible poses for the tag, along with
+   * the object-space error of each.
+   *
+   * <p>This uses the homography method described in [1] for the initial estimate. Then Orthogonal
+   * Iteration [2] is used to refine this estimate. Then [3] is used to find a potential second
+   * local minima and Orthogonal Iteration is used to refine this second estimate.
+   *
+   * <p>[1]: E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in 2011 IEEE
+   * International Conference on Robotics and Automation, May 2011, pp. 3400–3407.
+   *
+   * <p>[2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose estimation from
+   * video images," in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 22, no.
+   * 6, pp. 610-622, June 2000. doi: 10.1109/34.862199
+   *
+   * <p>[3]: Schweighofer and A. Pinz, "Robust Pose Estimation from a Planar Target," in IEEE
+   * Transactions on Pattern Analysis and Machine Intelligence, vol. 28, no. 12, pp. 2024-2030, Dec.
+   * 2006. doi: 10.1109/TPAMI.2006.252
+   *
+   * @param detection Tag detection
+   * @param nIters Number of iterations
+   * @return Initial and (possibly) second pose estimates
+   */
+  public AprilTagPoseEstimate estimateOrthogonalIteration(AprilTagDetection detection, int nIters) {
+    return estimateOrthogonalIteration(detection.getHomography(), detection.getCorners(), nIters);
+  }
+
+  /**
+   * Estimates the pose of the tag. This returns one or two possible poses for the tag, along with
+   * the object-space error of each.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @param nIters Number of iterations
+   * @return Initial and (possibly) second pose estimates
+   */
+  public AprilTagPoseEstimate estimateOrthogonalIteration(
+      double[] homography, double[] corners, int nIters) {
+    return AprilTagJNI.estimatePoseOrthogonalIteration(
+        homography,
+        corners,
+        m_config.tagSize,
+        m_config.fx,
+        m_config.fy,
+        m_config.cx,
+        m_config.cy,
+        nIters);
+  }
+
+  /**
+   * Estimates tag pose. This method is an easier to use interface to
+   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the pose with the lower
+   * object-space error.
+   *
+   * @param detection Tag detection
+   * @return Pose estimate
+   */
+  public Transform3d estimate(AprilTagDetection detection) {
+    return estimate(detection.getHomography(), detection.getCorners());
+  }
+
+  /**
+   * Estimates tag pose. This method is an easier to use interface to
+   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the pose with the lower
+   * object-space error.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @return Pose estimate
+   */
+  public Transform3d estimate(double[] homography, double[] corners) {
+    return AprilTagJNI.estimatePose(
+        homography, corners, m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy);
+  }
+
+  private final Config m_config;
+}

apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java

@@ -4,10 +4,13 @@
 
 package edu.wpi.first.apriltag.jni;
 
+import edu.wpi.first.apriltag.AprilTagDetection;
+import edu.wpi.first.apriltag.AprilTagDetector;
+import edu.wpi.first.apriltag.AprilTagPoseEstimate;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.util.RuntimeLoader;
 import java.io.IOException;
 import java.util.concurrent.atomic.AtomicBoolean;
-import org.opencv.core.Mat;
 
 public class AprilTagJNI {
   static boolean libraryLoaded = false;
@@ -41,49 +44,47 @@ public class AprilTagJNI {
     }
   }
 
-  // Returns a pointer to a apriltag_detector_t
-  public static native long aprilTagCreate(
-      String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges);
+  public static native long createDetector();
 
-  // Destroy and free a previously created detector.
-  public static native void aprilTagDestroy(long detector);
+  public static native void destroyDetector(long det);
 
-  private static native Object[] aprilTagDetectInternal(
-      long detector,
-      long imgAddr,
-      int rows,
-      int cols,
-      boolean doPoseEstimation,
-      double tagWidth,
+  public static native void setDetectorConfig(long det, AprilTagDetector.Config config);
+
+  public static native AprilTagDetector.Config getDetectorConfig(long det);
+
+  public static native void setDetectorQTP(
+      long det, AprilTagDetector.QuadThresholdParameters params);
+
+  public static native AprilTagDetector.QuadThresholdParameters getDetectorQTP(long det);
+
+  public static native boolean addFamily(long det, String fam, int bitsCorrected);
+
+  public static native void removeFamily(long det, String fam);
+
+  public static native void clearFamilies(long det);
+
+  public static native AprilTagDetection[] detect(
+      long det, int width, int height, int stride, long bufAddr);
+
+  public static native Transform3d estimatePoseHomography(
+      double[] homography, double tagSize, double fx, double fy, double cx, double cy);
+
+  public static native AprilTagPoseEstimate estimatePoseOrthogonalIteration(
+      double[] homography,
+      double[] corners,
+      double tagSize,
       double fx,
       double fy,
       double cx,
       double cy,
       int nIters);
 
-  // Detect targets given a GRAY frame. Returns a pointer toa zarray
-  public static DetectionResult[] aprilTagDetect(
-      long detector,
-      Mat img,
-      boolean doPoseEstimation,
-      double tagWidth,
+  public static native Transform3d estimatePose(
+      double[] homography,
+      double[] corners,
+      double tagSize,
       double fx,
       double fy,
       double cx,
-      double cy,
-      int nIters) {
-    return (DetectionResult[])
-        aprilTagDetectInternal(
-            detector,
-            img.dataAddr(),
-            img.rows(),
-            img.cols(),
-            doPoseEstimation,
-            tagWidth,
-            fx,
-            fy,
-            cx,
-            cy,
-            nIters);
-  }
+      double cy);
 }

apriltag/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java

@@ -1,226 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.apriltag.jni;
-
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
-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.N3;
-import java.util.Arrays;
-import org.ejml.data.DMatrixRMaj;
-import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
-import org.ejml.simple.SimpleMatrix;
-
-public class DetectionResult {
-  public int getId() {
-    return m_id;
-  }
-
-  public int getHamming() {
-    return m_hamming;
-  }
-
-  public float getDecisionMargin() {
-    return m_decisionMargin;
-  }
-
-  public void setDecisionMargin(float decisionMargin) {
-    this.m_decisionMargin = decisionMargin;
-  }
-
-  @SuppressWarnings("PMD.MethodReturnsInternalArray")
-  public double[] getHomography() {
-    return m_homography;
-  }
-
-  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
-  public void setHomography(double[] homography) {
-    this.m_homography = homography;
-  }
-
-  public double getCenterX() {
-    return m_centerX;
-  }
-
-  public void setCenterX(double centerX) {
-    this.m_centerX = centerX;
-  }
-
-  public double getCenterY() {
-    return m_centerY;
-  }
-
-  public void setCenterY(double centerY) {
-    this.m_centerY = centerY;
-  }
-
-  @SuppressWarnings("PMD.MethodReturnsInternalArray")
-  public double[] getCorners() {
-    return m_corners;
-  }
-
-  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
-  public void setCorners(double[] corners) {
-    this.m_corners = corners;
-  }
-
-  public double getError1() {
-    return m_error1;
-  }
-
-  public double getError2() {
-    return m_error2;
-  }
-
-  public Transform3d getPoseResult1() {
-    return m_poseResult1;
-  }
-
-  public Transform3d getPoseResult2() {
-    return m_poseResult2;
-  }
-
-  private final int m_id;
-  private final int m_hamming;
-  private float m_decisionMargin;
-  private double[] m_homography;
-  private double m_centerX;
-  private double m_centerY;
-  private double[] m_corners;
-
-  private final Transform3d m_poseResult1;
-  private final double m_error1;
-  private final Transform3d m_poseResult2;
-  private final double m_error2;
-
-  /**
-   * Constructs a new detection result. Used from JNI.
-   *
-   * @param id id
-   * @param hamming hamming
-   * @param decisionMargin dm
-   * @param homography homography
-   * @param centerX centerX
-   * @param centerY centerY
-   * @param corners corners
-   * @param pose1TransArr pose1TransArr
-   * @param pose1RotArr pose1RotArr
-   * @param err1 err1
-   * @param pose2TransArr pose2TransArr
-   * @param pose2RotArr pose2RotArr
-   * @param err2 err2
-   */
-  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
-  public DetectionResult(
-      int id,
-      int hamming,
-      float decisionMargin,
-      double[] homography,
-      double centerX,
-      double centerY,
-      double[] corners,
-      double[] pose1TransArr,
-      double[] pose1RotArr,
-      double err1,
-      double[] pose2TransArr,
-      double[] pose2RotArr,
-      double err2) {
-    this.m_id = id;
-    this.m_hamming = hamming;
-    this.m_decisionMargin = decisionMargin;
-    this.m_homography = homography;
-    this.m_centerX = centerX;
-    this.m_centerY = centerY;
-    this.m_corners = corners;
-
-    this.m_error1 = err1;
-    this.m_poseResult1 =
-        new Transform3d(
-            new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]),
-            new Rotation3d(
-                orthogonalizeRotationMatrix(
-                    new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr))));
-    this.m_error2 = err2;
-    this.m_poseResult2 =
-        new Transform3d(
-            new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]),
-            new Rotation3d(
-                orthogonalizeRotationMatrix(
-                    new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr))));
-  }
-
-  /**
-   * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
-   * ambiguous.
-   *
-   * @return The ratio of pose reprojection errors.
-   */
-  public double getPoseAmbiguity() {
-    var min = Math.min(m_error1, m_error2);
-    var max = Math.max(m_error1, m_error2);
-
-    if (max > 0) {
-      return min / max;
-    } else {
-      return -1;
-    }
-  }
-
-  @Override
-  public String toString() {
-    return "DetectionResult [centerX="
-        + m_centerX
-        + ", centerY="
-        + m_centerY
-        + ", corners="
-        + Arrays.toString(m_corners)
-        + ", decisionMargin="
-        + m_decisionMargin
-        + ", error1="
-        + m_error1
-        + ", error2="
-        + m_error2
-        + ", hamming="
-        + m_hamming
-        + ", homography="
-        + Arrays.toString(m_homography)
-        + ", id="
-        + m_id
-        + ", poseResult1="
-        + m_poseResult1
-        + ", poseResult2="
-        + m_poseResult2
-        + "]";
-  }
-
-  private static Matrix<N3, N3> orthogonalizeRotationMatrix(Matrix<N3, N3> input) {
-    var a = DecompositionFactory_DDRM.qr(3, 3);
-    if (!a.decompose(input.getStorage().getDDRM())) {
-      // best we can do is return the input
-      return input;
-    }
-
-    // Grab results (thanks for this _great_ api, EJML)
-    var Q = new DMatrixRMaj(3, 3);
-    var R = new DMatrixRMaj(3, 3);
-    a.getQ(Q, false);
-    a.getR(R, false);
-
-    // Fix signs in R if they're < 0 so it's close to an identity matrix
-    // (our QR decomposition implementation sometimes flips the signs of columns)
-    for (int colR = 0; colR < 3; ++colR) {
-      if (R.get(colR, colR) < 0) {
-        for (int rowQ = 0; rowQ < 3; ++rowQ) {
-          Q.set(rowQ, colR, -Q.get(rowQ, colR));
-        }
-      }
-    }
-
-    return new Matrix<>(new SimpleMatrix(Q));
-  }
-}

apriltag/src/main/native/cpp/AprilTagDetection.cpp

@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/apriltag/AprilTagDetection.h"
+
+#include <type_traits>
+
+#ifdef _WIN32
+#pragma warning(disable : 4200)
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wc99-extensions"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
+
+#include "apriltag.h"
+
+using namespace frc;
+
+static_assert(sizeof(AprilTagDetection) == sizeof(apriltag_detection_t),
+              "structure sizes don't match");
+static_assert(std::is_standard_layout_v<AprilTagDetection>,
+              "AprilTagDetection is not standard layout?");
+
+std::string_view AprilTagDetection::GetFamily() const {
+  return static_cast<const apriltag_family_t*>(family)->name;
+}
+
+std::span<const double, 9> AprilTagDetection::GetHomography() const {
+  return std::span<const double, 9>{static_cast<matd_t*>(H)->data, 9};
+}
+
+Eigen::Matrix3d AprilTagDetection::GetHomographyMatrix() const {
+  return Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>{
+      static_cast<matd_t*>(H)->data};
+}

apriltag/src/main/native/cpp/AprilTagDetector.cpp

@@ -0,0 +1,200 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/apriltag/AprilTagDetector.h"
+
+#include <cmath>
+#include <numbers>
+
+#ifdef _WIN32
+#pragma warning(disable : 4200)
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wc99-extensions"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
+
+#include "apriltag.h"
+#include "tag16h5.h"
+#include "tag25h9.h"
+#include "tag36h11.h"
+#include "tagCircle21h7.h"
+#include "tagCircle49h12.h"
+#include "tagCustom48h12.h"
+#include "tagStandard41h12.h"
+#include "tagStandard52h13.h"
+
+using namespace frc;
+
+AprilTagDetector::Results::Results(void* impl, const private_init&)
+    : span{reinterpret_cast<AprilTagDetection**>(
+               static_cast<zarray_t*>(impl)->data),
+           static_cast<size_t>(static_cast<zarray_t*>(impl)->size)},
+      m_impl{impl} {}
+
+AprilTagDetector::Results& AprilTagDetector::Results::operator=(Results&& rhs) {
+  Destroy();
+  m_impl = rhs.m_impl;
+  rhs.m_impl = nullptr;
+  return *this;
+}
+
+void AprilTagDetector::Results::Destroy() {
+  if (m_impl) {
+    apriltag_detections_destroy(static_cast<zarray_t*>(m_impl));
+  }
+}
+
+AprilTagDetector::AprilTagDetector() : m_impl{apriltag_detector_create()} {}
+
+AprilTagDetector& AprilTagDetector::operator=(AprilTagDetector&& rhs) {
+  Destroy();
+  m_impl = rhs.m_impl;
+  rhs.m_impl = nullptr;
+  m_families = std::move(rhs.m_families);
+  rhs.m_families.clear();
+  m_qtpCriticalAngle = rhs.m_qtpCriticalAngle;
+  return *this;
+}
+
+void AprilTagDetector::SetConfig(const Config& config) {
+  auto& impl = *static_cast<apriltag_detector_t*>(m_impl);
+  impl.nthreads = config.numThreads;
+  impl.quad_decimate = config.quadDecimate;
+  impl.quad_sigma = config.quadSigma;
+  impl.refine_edges = config.refineEdges;
+  impl.decode_sharpening = config.decodeSharpening;
+  impl.debug = config.debug;
+}
+
+AprilTagDetector::Config AprilTagDetector::GetConfig() const {
+  auto& impl = *static_cast<apriltag_detector_t*>(m_impl);
+  return {
+      .numThreads = impl.nthreads,
+      .quadDecimate = impl.quad_decimate,
+      .quadSigma = impl.quad_sigma,
+      .refineEdges = impl.refine_edges,
+      .decodeSharpening = impl.decode_sharpening,
+      .debug = impl.debug,
+  };
+}
+
+void AprilTagDetector::SetQuadThresholdParameters(
+    const QuadThresholdParameters& params) {
+  auto& qtp = static_cast<apriltag_detector_t*>(m_impl)->qtp;
+  qtp.min_cluster_pixels = params.minClusterPixels;
+  qtp.max_nmaxima = params.maxNumMaxima;
+  qtp.critical_rad = params.criticalAngle.value();
+  qtp.cos_critical_rad = std::cos(params.criticalAngle.value());
+  qtp.max_line_fit_mse = params.maxLineFitMSE;
+  qtp.min_white_black_diff = params.minWhiteBlackDiff;
+  qtp.deglitch = params.deglitch;
+
+  m_qtpCriticalAngle = params.criticalAngle;
+}
+
+AprilTagDetector::QuadThresholdParameters
+AprilTagDetector::GetQuadThresholdParameters() const {
+  auto& qtp = static_cast<apriltag_detector_t*>(m_impl)->qtp;
+  return {
+      .minClusterPixels = qtp.min_cluster_pixels,
+      .maxNumMaxima = qtp.max_nmaxima,
+      .criticalAngle = m_qtpCriticalAngle,
+      .maxLineFitMSE = qtp.max_line_fit_mse,
+      .minWhiteBlackDiff = qtp.min_white_black_diff,
+      .deglitch = qtp.deglitch != 0,
+  };
+}
+
+bool AprilTagDetector::AddFamily(std::string_view fam, int bitsCorrected) {
+  auto& data = m_families[fam];
+  if (data) {
+    return true;  // already detecting
+  }
+  // create the family
+  if (fam == "tag16h5") {
+    data = tag16h5_create();
+  } else if (fam == "tag25h9") {
+    data = tag25h9_create();
+  } else if (fam == "tag36h11") {
+    data = tag36h11_create();
+  } else if (fam == "tagCircle21h7") {
+    data = tagCircle21h7_create();
+  } else if (fam == "tagCircle49h12") {
+    data = tagCircle49h12_create();
+  } else if (fam == "tagStandard41h12") {
+    data = tagStandard41h12_create();
+  } else if (fam == "tagStandard52h13") {
+    data = tagStandard52h13_create();
+  } else if (fam == "tagCustom48h12") {
+    data = tagCustom48h12_create();
+  }
+  if (!data) {
+    m_families.erase(fam);  // don't keep null value
+    return false;           // can't add
+  }
+  apriltag_detector_add_family_bits(static_cast<apriltag_detector_t*>(m_impl),
+                                    static_cast<apriltag_family_t*>(data),
+                                    bitsCorrected);
+  return true;
+}
+
+void AprilTagDetector::RemoveFamily(std::string_view fam) {
+  auto it = m_families.find(fam);
+  if (it != m_families.end()) {
+    apriltag_detector_remove_family(
+        static_cast<apriltag_detector_t*>(m_impl),
+        static_cast<apriltag_family_t*>(it->second));
+    DestroyFamily(it->getKey(), it->second);
+    m_families.erase(it);
+  }
+}
+
+void AprilTagDetector::ClearFamilies() {
+  apriltag_detector_clear_families(static_cast<apriltag_detector_t*>(m_impl));
+  DestroyFamilies();
+  m_families.clear();
+}
+
+AprilTagDetector::Results AprilTagDetector::Detect(int width, int height,
+                                                   int stride, uint8_t* buf) {
+  image_u8_t img{width, height, stride, buf};
+  return {
+      apriltag_detector_detect(static_cast<apriltag_detector_t*>(m_impl), &img),
+      Results::private_init{}};
+}
+
+void AprilTagDetector::Destroy() {
+  if (m_impl) {
+    apriltag_detector_destroy(static_cast<apriltag_detector_t*>(m_impl));
+  }
+  DestroyFamilies();
+}
+
+void AprilTagDetector::DestroyFamilies() {
+  for (auto&& entry : m_families) {
+    DestroyFamily(entry.getKey(), entry.second);
+  }
+}
+
+void AprilTagDetector::DestroyFamily(std::string_view name, void* data) {
+  auto fam = static_cast<apriltag_family_t*>(data);
+  if (name == "tag16h5") {
+    tag16h5_destroy(fam);
+  } else if (name == "tag25h9") {
+    tag25h9_destroy(fam);
+  } else if (name == "tag36h11") {
+    tag36h11_destroy(fam);
+  } else if (name == "tagCircle21h7") {
+    tagCircle21h7_destroy(fam);
+  } else if (name == "tagCircle49h12") {
+    tagCircle49h12_destroy(fam);
+  } else if (name == "tagStandard41h12") {
+    tagStandard41h12_destroy(fam);
+  } else if (name == "tagStandard52h13") {
+    tagStandard52h13_destroy(fam);
+  } else if (name == "tagCustom48h12") {
+    tagCustom48h12_destroy(fam);
+  }
+}

apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp

@@ -29,7 +29,7 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
     m_apriltags[tag.ID] = tag;
   }
   m_fieldWidth = units::meter_t{json.at("field").at("width").get<double>()};
-  m_fieldLength = units::meter_t{json.at("field").at("height").get<double>()};
+  m_fieldLength = units::meter_t{json.at("field").at("length").get<double>()};
 }
 
 AprilTagFieldLayout::AprilTagFieldLayout(std::vector<AprilTag> apriltags,

apriltag/src/main/native/cpp/AprilTagFields.cpp

@@ -10,6 +10,7 @@ namespace frc {
 
 // C++ generated from resource files
 std::string_view GetResource_2022_rapidreact_json();
+std::string_view GetResource_2023_chargedup_json();
 
 AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
   std::string_view fieldString;
@@ -17,6 +18,9 @@ AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
     case AprilTagField::k2022RapidReact:
       fieldString = GetResource_2022_rapidreact_json();
       break;
+    case AprilTagField::k2023ChargedUp:
+      fieldString = GetResource_2023_chargedup_json();
+      break;
     case AprilTagField::kNumFields:
       throw std::invalid_argument("Invalid Field");
   }

apriltag/src/main/native/cpp/AprilTagPoseEstimate.cpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/apriltag/AprilTagPoseEstimate.h"
+
+#include <algorithm>
+
+using namespace frc;
+
+double AprilTagPoseEstimate::GetAmbiguity() const {
+  auto min = (std::min)(error1, error2);
+  auto max = (std::max)(error1, error2);
+
+  if (max > 0) {
+    return min / max;
+  } else {
+    return -1;
+  }
+}

apriltag/src/main/native/cpp/AprilTagPoseEstimator.cpp

@@ -0,0 +1,154 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/apriltag/AprilTagPoseEstimator.h"
+
+#include <Eigen/QR>
+
+#include "frc/apriltag/AprilTagDetection.h"
+
+#ifdef _WIN32
+#pragma warning(disable : 4200)
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wc99-extensions"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
+
+#include "apriltag.h"
+#include "apriltag_pose.h"
+
+using namespace frc;
+
+static Eigen::Matrix3d OrthogonalizeRotationMatrix(
+    const Eigen::Matrix3d& input) {
+  Eigen::HouseholderQR<Eigen::Matrix3d> qr{input};
+
+  Eigen::Matrix3d Q = qr.householderQ();
+  Eigen::Matrix3d R = qr.matrixQR().triangularView<Eigen::Upper>();
+
+  // Fix signs in R if they're < 0 so it's close to an identity matrix
+  // (our QR decomposition implementation sometimes flips the signs of
+  // columns)
+  for (int colR = 0; colR < 3; ++colR) {
+    if (R(colR, colR) < 0) {
+      for (int rowQ = 0; rowQ < 3; ++rowQ) {
+        Q(rowQ, colR) = -Q(rowQ, colR);
+      }
+    }
+  }
+
+  return Q;
+}
+
+static Transform3d MakePose(const apriltag_pose_t& pose) {
+  if (!pose.R || !pose.t) {
+    return {};
+  }
+  return {Translation3d{units::meter_t{pose.t->data[0]},
+                        units::meter_t{pose.t->data[1]},
+                        units::meter_t{pose.t->data[2]}},
+          Rotation3d{OrthogonalizeRotationMatrix(
+              Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>{
+                  pose.R->data})}};
+}
+
+static apriltag_detection_info_t MakeDetectionInfo(
+    const apriltag_detection_t* det,
+    const AprilTagPoseEstimator::Config& config) {
+  return {const_cast<apriltag_detection_t*>(det),
+          config.tagSize.value(),
+          config.fx,
+          config.fy,
+          config.cx,
+          config.cy};
+}
+
+static apriltag_detection_t MakeBasicDet(
+    std::span<const double, 9> homography,
+    const std::span<const double, 8>* corners) {
+  apriltag_detection_t detection;
+  detection.H = matd_create(3, 3);
+  std::memcpy(detection.H->data, homography.data(), 9 * sizeof(double));
+  if (corners) {
+    for (int i = 0; i < 4; i++) {
+      detection.p[i][0] = (*corners)[i * 2];
+      detection.p[i][1] = (*corners)[i * 2 + 1];
+    }
+  }
+  return detection;
+}
+
+static Transform3d DoEstimateHomography(
+    const apriltag_detection_t* detection,
+    const AprilTagPoseEstimator::Config& config) {
+  auto info = MakeDetectionInfo(detection, config);
+  apriltag_pose_t pose;
+  estimate_pose_for_tag_homography(&info, &pose);
+  return MakePose(pose);
+}
+
+Transform3d AprilTagPoseEstimator::EstimateHomography(
+    const AprilTagDetection& detection) const {
+  return DoEstimateHomography(
+      reinterpret_cast<const apriltag_detection_t*>(&detection), m_config);
+}
+
+Transform3d AprilTagPoseEstimator::EstimateHomography(
+    std::span<const double, 9> homography) const {
+  auto detection = MakeBasicDet(homography, nullptr);
+  auto rv = DoEstimateHomography(&detection, m_config);
+  matd_destroy(detection.H);
+  return rv;
+}
+
+static AprilTagPoseEstimate DoEstimateOrthogonalIteration(
+    const apriltag_detection_t* detection,
+    const AprilTagPoseEstimator::Config& config, int nIters) {
+  auto info = MakeDetectionInfo(detection, config);
+  apriltag_pose_t pose1, pose2;
+  double err1, err2;
+  estimate_tag_pose_orthogonal_iteration(&info, &err1, &pose1, &err2, &pose2,
+                                         nIters);
+  return {MakePose(pose1), MakePose(pose2), err1, err2};
+}
+
+AprilTagPoseEstimate AprilTagPoseEstimator::EstimateOrthogonalIteration(
+    const AprilTagDetection& detection, int nIters) const {
+  return DoEstimateOrthogonalIteration(
+      reinterpret_cast<const apriltag_detection_t*>(&detection), m_config,
+      nIters);
+}
+
+AprilTagPoseEstimate AprilTagPoseEstimator::EstimateOrthogonalIteration(
+    std::span<const double, 9> homography, std::span<const double, 8> corners,
+    int nIters) const {
+  auto detection = MakeBasicDet(homography, &corners);
+  auto rv = DoEstimateOrthogonalIteration(&detection, m_config, nIters);
+  matd_destroy(detection.H);
+  return rv;
+}
+
+static Transform3d DoEstimate(const apriltag_detection_t* detection,
+                              const AprilTagPoseEstimator::Config& config) {
+  auto info = MakeDetectionInfo(detection, config);
+  apriltag_pose_t pose;
+  estimate_tag_pose(&info, &pose);
+  return MakePose(pose);
+}
+
+Transform3d AprilTagPoseEstimator::Estimate(
+    const AprilTagDetection& detection) const {
+  return DoEstimate(reinterpret_cast<const apriltag_detection_t*>(&detection),
+                    m_config);
+}
+
+Transform3d AprilTagPoseEstimator::Estimate(
+    std::span<const double, 9> homography,
+    std::span<const double, 8> corners) const {
+  auto detection = MakeBasicDet(homography, &corners);
+  auto rv = DoEstimate(&detection, m_config);
+  matd_destroy(detection.H);
+  return rv;
+}

apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp

@@ -2,319 +2,594 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <cstdio>
+#include <cstring>
+
 #include <wpi/jni_util.h>
 
 #include "edu_wpi_first_apriltag_jni_AprilTagJNI.h"
+#include "frc/apriltag/AprilTagDetector.h"
+#include "frc/apriltag/AprilTagPoseEstimator.h"
 
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4200)
-#endif
-
-#if defined(__GNUC__)
-#pragma GCC diagnostic ignored "-Wpedantic"
-#endif
-#include "apriltag.h"
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
-
-#include "tag36h11.h"
-#include "tag25h9.h"
-#include "tag16h5.h"
-#include "tagCircle21h7.h"
-#include "tagCircle49h12.h"
-#include "tagCustom48h12.h"
-#include "tagStandard41h12.h"
-#include "tagStandard52h13.h"
-#include "apriltag_pose.h"
-
-#include <vector>
-#include <algorithm>
-#include <cmath>
-
+using namespace frc;
 using namespace wpi::java;
 
-struct DetectorState {
-  int id;
-  apriltag_detector_t* td;
-  apriltag_family_t* tf;
-  void (*tf_destroy)(apriltag_family_t*);
-};
+static JavaVM* jvm = nullptr;
 
-static std::vector<DetectorState> detectors;
+static JClass detectionCls;
+static JClass detectorConfigCls;
+static JClass detectorQTPCls;
+static JClass poseEstimateCls;
+static JClass quaternionCls;
+static JClass rotation3dCls;
+static JClass transform3dCls;
+static JClass translation3dCls;
+static JException illegalArgEx;
+static JException nullPointerEx;
+
+static const JClassInit classes[] = {
+    {"edu/wpi/first/apriltag/AprilTagDetection", &detectionCls},
+    {"edu/wpi/first/apriltag/AprilTagDetector$Config", &detectorConfigCls},
+    {"edu/wpi/first/apriltag/AprilTagDetector$QuadThresholdParameters",
+     &detectorQTPCls},
+    {"edu/wpi/first/apriltag/AprilTagPoseEstimate", &poseEstimateCls},
+    {"edu/wpi/first/math/geometry/Quaternion", &quaternionCls},
+    {"edu/wpi/first/math/geometry/Rotation3d", &rotation3dCls},
+    {"edu/wpi/first/math/geometry/Transform3d", &transform3dCls},
+    {"edu/wpi/first/math/geometry/Translation3d", &translation3dCls}};
+
+static const JExceptionInit exceptions[] = {
+    {"java/lang/IllegalArgumentException", &illegalArgEx},
+    {"java/lang/NullPointerException", &nullPointerEx}};
 
 extern "C" {
 
-/*
- * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
- * Method:    aprilTagCreate
- * Signature: (Ljava/lang/String;DDIZZ)J
- */
-JNIEXPORT jlong JNICALL
-Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagCreate
-  (JNIEnv* env, jclass cls, jstring jstr, jdouble decimate, jdouble blur,
-   jint threads, jboolean debug, jboolean refine_edges)
-{
-  // Initialize tag detector with options
-  apriltag_family_t* tf = nullptr;
-  // const char *famname = fam;
-  const char* famname = env->GetStringUTFChars(jstr, nullptr);
-
-  void (*tf_destroy_func)(apriltag_family_t*);
-
-  if (!strcmp(famname, "tag36h11")) {
-    tf = tag36h11_create();
-    tf_destroy_func = tag36h11_destroy;
-  } else if (!strcmp(famname, "tag25h9")) {
-    tf = tag25h9_create();
-    tf_destroy_func = tag25h9_destroy;
-  } else if (!strcmp(famname, "tag16h5")) {
-    tf = tag16h5_create();
-    tf_destroy_func = tag16h5_destroy;
-  } else if (!strcmp(famname, "tagCircle21h7")) {
-    tf = tagCircle21h7_create();
-    tf_destroy_func = tagCircle21h7_destroy;
-  } else if (!strcmp(famname, "tagCircle49h12")) {
-    tf = tagCircle49h12_create();
-    tf_destroy_func = tagCircle49h12_destroy;
-  } else if (!strcmp(famname, "tagStandard41h12")) {
-    tf = tagStandard41h12_create();
-    tf_destroy_func = tagStandard41h12_destroy;
-  } else if (!strcmp(famname, "tagStandard52h13")) {
-    tf = tagStandard52h13_create();
-    tf_destroy_func = tagStandard52h13_destroy;
-  } else if (!strcmp(famname, "tagCustom48h12")) {
-    tf = tagCustom48h12_create();
-    tf_destroy_func = tagCustom48h12_destroy;
-  } else {
-    std::printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
-    env->ReleaseStringUTFChars(jstr, famname);
-    return 0;
-  }
-
-  apriltag_detector_t* td = apriltag_detector_create();
-  apriltag_detector_add_family(td, tf);
-  td->quad_decimate = static_cast<float>(decimate);
-  td->quad_sigma = static_cast<float>(blur);
-  td->nthreads = threads;
-  td->debug = debug;
-  td->refine_edges = refine_edges;
-
-  env->ReleaseStringUTFChars(jstr, famname);
-
-  // std::printf("Looking for max\n");
-  auto max = std::max_element(detectors.begin(), detectors.end(),
-                              [](DetectorState& a, DetectorState& b) {
-                                return a.id < b.id;
-                              });  // detectors.size();
-  int index = 0;
-  if (max != detectors.end())
-    index = max->id + 1;
-  detectors.push_back({index, td, tf, tf_destroy_func});
-  std::printf("Created detector at idx %i\n", index);
-  return (jlong)index;
-}
-
-static JClass detectionClass;
-
 JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
+  jvm = vm;
+
   JNIEnv* env;
   if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return JNI_ERR;
   }
 
-  detectionClass = JClass(env, "edu/wpi/first/apriltag/jni/DetectionResult");
+  // Cache references to classes
+  for (auto& c : classes) {
+    *c.cls = JClass(env, c.name);
+    if (!*c.cls) {
+      std::fprintf(stderr, "could not load class %s\n", c.name);
+      return JNI_ERR;
+    }
+  }
 
-  if (!detectionClass) {
-    std::printf("Couldn't find class!");
-    return JNI_ERR;
+  for (auto& c : exceptions) {
+    *c.cls = JException(env, c.name);
+    if (!*c.cls) {
+      std::fprintf(stderr, "could not load exception %s\n", c.name);
+      return JNI_ERR;
+    }
   }
 
   return JNI_VERSION_1_6;
 }
 
-static jobject MakeJObject(JNIEnv* env, const apriltag_detection_t* detect,
-                           apriltag_pose_t& pose1, apriltag_pose_t& pose2,
-                           double error1, double error2) {
-  // Constructor signature must match Java! I = int, F = float, [D = double
-  // array
-  static jmethodID constructor =
-      env->GetMethodID(detectionClass, "<init>", "(IIF[DDD[D[D[DD[D[DD)V");
+JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return;
+  }
+  // Delete global references
+  for (auto& c : classes) {
+    c.cls->free(env);
+  }
+  for (auto& c : exceptions) {
+    c.cls->free(env);
+  }
+  jvm = nullptr;
+}
 
+}  // extern "C"
+
+//
+// Conversions from Java to C++ objects
+//
+
+static AprilTagDetector::Config FromJavaDetectorConfig(JNIEnv* env,
+                                                       jobject jconfig) {
+  if (!jconfig) {
+    return {};
+  }
+#define FIELD(name, sig)                                          \
+  static jfieldID name##Field = nullptr;                          \
+  if (!name##Field) {                                             \
+    name##Field = env->GetFieldID(detectorConfigCls, #name, sig); \
+  }
+
+  FIELD(numThreads, "I");
+  FIELD(quadDecimate, "F");
+  FIELD(quadSigma, "F");
+  FIELD(refineEdges, "Z");
+  FIELD(decodeSharpening, "D");
+  FIELD(debug, "Z");
+
+#undef FIELD
+
+#define FIELD(ctype, jtype, name) \
+  .name = static_cast<ctype>(env->Get##jtype##Field(jconfig, name##Field))
+
+  return {
+      FIELD(int, Int, numThreads),
+      FIELD(float, Float, quadDecimate),
+      FIELD(float, Float, quadSigma),
+      FIELD(bool, Boolean, refineEdges),
+      FIELD(double, Double, decodeSharpening),
+      FIELD(bool, Boolean, debug),
+  };
+
+#undef GET
+#undef FIELD
+}
+
+static AprilTagDetector::QuadThresholdParameters FromJavaDetectorQTP(
+    JNIEnv* env, jobject jparams) {
+  if (!jparams) {
+    return {};
+  }
+#define FIELD(name, sig)                                       \
+  static jfieldID name##Field = nullptr;                       \
+  if (!name##Field) {                                          \
+    name##Field = env->GetFieldID(detectorQTPCls, #name, sig); \
+  }
+
+  FIELD(minClusterPixels, "I");
+  FIELD(maxNumMaxima, "I");
+  FIELD(criticalAngle, "D");
+  FIELD(maxLineFitMSE, "F");
+  FIELD(minWhiteBlackDiff, "I");
+  FIELD(deglitch, "Z");
+
+#undef FIELD
+
+#define FIELD(ctype, jtype, name) \
+  .name = static_cast<ctype>(env->Get##jtype##Field(jparams, name##Field))
+
+  return {
+      FIELD(int, Int, minClusterPixels),
+      FIELD(int, Int, maxNumMaxima),
+      .criticalAngle = units::radian_t{static_cast<double>(
+          env->GetDoubleField(jparams, criticalAngleField))},
+      FIELD(float, Float, maxLineFitMSE),
+      FIELD(int, Int, minWhiteBlackDiff),
+      FIELD(bool, Boolean, deglitch),
+  };
+
+#undef GET
+#undef FIELD
+}
+
+//
+// Conversions from C++ to Java objects
+//
+
+static jobject MakeJObject(JNIEnv* env, const AprilTagDetection& detect) {
+  static jmethodID constructor = env->GetMethodID(
+      detectionCls, "<init>", "(Ljava/lang/String;IIF[DDD[D)V");
   if (!constructor) {
     return nullptr;
   }
 
-  if (!detect) {
-    return nullptr;
-  }
+  JLocal<jstring> fam{env, MakeJString(env, detect.GetFamily())};
 
-  // We have to copy the homography matrix and coners into jdoubles
-  jdouble h[9];  // = new jdouble[9]{};
-  for (int i = 0; i < 9; i++) {
-    h[i] = detect->H->data[i];
-  }
+  auto homography = detect.GetHomography();
+  JLocal<jdoubleArray> harr{
+      env, MakeJDoubleArray(
+               env, {reinterpret_cast<const jdouble*>(homography.data()),
+                     homography.size()})};
 
-  jdouble corners[8];  // = new jdouble[8]{};
-  for (int i = 0; i < 4; i++) {
-    corners[i * 2] = detect->p[i][0];
-    corners[i * 2 + 1] = detect->p[i][1];
-  }
+  double cornersBuf[8];
+  auto corners = detect.GetCorners(cornersBuf);
+  JLocal<jdoubleArray> carr{
+      env,
+      MakeJDoubleArray(env, {reinterpret_cast<const jdouble*>(corners.data()),
+                             corners.size()})};
 
-  jdoubleArray harr = MakeJDoubleArray(env, {h, 9});
-  jdoubleArray carr = MakeJDoubleArray(env, {corners, 8});
+  auto center = detect.GetCenter();
 
-  // The rotation of the target is encoded as a 3 by 3 rotation matrix, we'll
-  // convert to a row-major array
-  jdouble pose1RotMat[9] = {0};
-  jdouble pose2RotMat[9] = {0};
-
-  for (int i = 0; i < 9; i++) {
-    if (pose1.R) {
-      pose1RotMat[i] = pose1.R->data[i];
-    }
-    if (pose2.R) {
-      pose2RotMat[i] = pose2.R->data[i];
-    }
-  }
-
-  // And translation a 3x1 vector (todo check axis order)
-  jdouble pose1Trans[3] = {0};
-  jdouble pose2Trans[3] = {0};
-  for (int i = 0; i < 3; i++) {
-    if (pose1.t) {
-      pose1Trans[i] = pose1.t->data[i];
-    }
-    if (pose2.t) {
-      pose2Trans[i] = pose2.t->data[i];
-    }
-  }
-
-  jdoubleArray pose1rotArr = MakeJDoubleArray(env, {pose1RotMat, 9});
-  jdoubleArray pose2rotArr = MakeJDoubleArray(env, {pose2RotMat, 9});
-  jdoubleArray pose1transArr = MakeJDoubleArray(env, {pose1Trans, 3});
-  jdoubleArray pose2transArr = MakeJDoubleArray(env, {pose2Trans, 3});
-  jdouble err1 = error1;
-  jdouble err2 = error2;
-
-  // Actually call the constructor
-  auto ret = env->NewObject(
-      detectionClass, constructor, (jint)detect->id, (jint)detect->hamming,
-      (jfloat)detect->decision_margin, harr, (jdouble)detect->c[0],
-      (jdouble)detect->c[1], carr, pose1transArr, pose1rotArr, err1,
-      pose2transArr, pose2rotArr, err2);
-
-  // TODO we don't seem to need this... or at least, it doesnt leak rn
-  // env->ReleaseDoubleArrayElements(harr, h, 0);
-  // env->ReleaseDoubleArrayElements(carr, corners, 0);
-
-  return ret;
+  return env->NewObject(detectionCls, constructor, fam.obj(),
+                        static_cast<jint>(detect.GetId()),
+                        static_cast<jint>(detect.GetHamming()),
+                        static_cast<jfloat>(detect.GetDecisionMargin()),
+                        harr.obj(), static_cast<jdouble>(center.x),
+                        static_cast<jdouble>(center.y), carr.obj());
 }
 
-/*
- * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
- * Method:    aprilTagDetectInternal
- * Signature: (JJIIZDDDDDI)[Ljava/lang/Object;
- */
-JNIEXPORT jobjectArray JNICALL
-Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagDetectInternal
-  (JNIEnv* env, jclass cls, jlong detectIdx, jlong pData, jint rows, jint cols,
-   jboolean doPoseEstimation, jdouble tagWidthMeters, jdouble fx, jdouble fy,
-   jdouble cx, jdouble cy, jint nIters)
-{
-  // No image, can't do anything
-  if (!pData) {
-    return nullptr;
-  }
-
-  // Make an image_u8_t header for the Mat data
-  image_u8_t im = {static_cast<int32_t>(cols), static_cast<int32_t>(rows),
-                   static_cast<int32_t>(cols),
-                   reinterpret_cast<uint8_t*>(pData)};
-
-  // Get our detector
-  auto state =
-      std::find_if(detectors.begin(), detectors.end(),
-                   [&](DetectorState& s) { return s.id == detectIdx; });
-  if (state == detectors.end()) {
-    return nullptr;
-  }
-
-  // And run the detector on our new image
-  zarray_t* detections = apriltag_detector_detect(state->td, &im);
-
-  int size = zarray_size(detections);
-
-  // Object array to return to Java
-  jobjectArray jarr = env->NewObjectArray(size, detectionClass, nullptr);
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const AprilTagDetection* const> arr) {
+  jobjectArray jarr = env->NewObjectArray(arr.size(), detectionCls, nullptr);
   if (!jarr) {
-    std::printf("Couldn't make array\n");
     return nullptr;
   }
-
-  // Global pose
-  apriltag_pose_t pose1;
-  std::memset(&pose1, 0, sizeof(pose1));
-
-  apriltag_pose_t pose2;
-  std::memset(&pose2, 0, sizeof(pose2));
-
-  // std::printf("Created array %llu! Got %i targets!\n", &jarr, size);
-  //  Add our detected targets to the array
-  for (int i = 0; i < size; ++i) {
-    apriltag_detection_t* det = nullptr;
-    zarray_get(detections, i, &det);
-
-    if (det != nullptr) {
-      double err1 =
-          HUGE_VAL;  // Should get overwritten if pose estimation is happening
-      double err2 = HUGE_VAL;
-      if (doPoseEstimation) {
-        // Feed results to the pose estimator
-        apriltag_detection_info_t info{det, tagWidthMeters, fx, fy, cx, cy};
-        estimate_tag_pose_orthogonal_iteration(&info, &err1, &pose1, &err2,
-                                               &pose2, nIters);
-      }
-
-      jobject obj = MakeJObject(env, det, pose1, pose2, err1, err2);
-
-      env->SetObjectArrayElement(jarr, i, obj);
-    }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, *arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
   }
-
-  // Now that stuff's in our Java-side array, we can clean up native memory
-  apriltag_detections_destroy(detections);
-
   return jarr;
 }
 
+static jobject MakeJObject(JNIEnv* env,
+                           const AprilTagDetector::Config& config) {
+  static jmethodID constructor =
+      env->GetMethodID(detectorConfigCls, "<init>", "(IFFZDZ)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  return env->NewObject(detectorConfigCls, constructor,
+                        static_cast<jint>(config.numThreads),
+                        static_cast<jfloat>(config.quadDecimate),
+                        static_cast<jfloat>(config.quadSigma),
+                        static_cast<jboolean>(config.refineEdges),
+                        static_cast<jdouble>(config.decodeSharpening),
+                        static_cast<jboolean>(config.debug));
+}
+
+static jobject MakeJObject(
+    JNIEnv* env, const AprilTagDetector::QuadThresholdParameters& params) {
+  static jmethodID constructor =
+      env->GetMethodID(detectorQTPCls, "<init>", "(IIDFIZ)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  return env->NewObject(detectorQTPCls, constructor,
+                        static_cast<jint>(params.minClusterPixels),
+                        static_cast<jint>(params.maxNumMaxima),
+                        static_cast<jdouble>(params.criticalAngle),
+                        static_cast<jfloat>(params.maxLineFitMSE),
+                        static_cast<jint>(params.minWhiteBlackDiff),
+                        static_cast<jboolean>(params.deglitch));
+}
+
+static jobject MakeJObject(JNIEnv* env, const Translation3d& xlate) {
+  static jmethodID constructor =
+      env->GetMethodID(translation3dCls, "<init>", "(DDD)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  return env->NewObject(
+      translation3dCls, constructor, static_cast<jdouble>(xlate.X()),
+      static_cast<jdouble>(xlate.Y()), static_cast<jdouble>(xlate.Z()));
+}
+
+static jobject MakeJObject(JNIEnv* env, const Quaternion& q) {
+  static jmethodID constructor =
+      env->GetMethodID(quaternionCls, "<init>", "(DDDD)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  return env->NewObject(quaternionCls, constructor, static_cast<jdouble>(q.W()),
+                        static_cast<jdouble>(q.X()),
+                        static_cast<jdouble>(q.Y()),
+                        static_cast<jdouble>(q.Z()));
+}
+
+static jobject MakeJObject(JNIEnv* env, const Rotation3d& rot) {
+  static jmethodID constructor = env->GetMethodID(
+      rotation3dCls, "<init>", "(Ledu/wpi/first/math/geometry/Quaternion;)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  JLocal<jobject> q{env, MakeJObject(env, rot.GetQuaternion())};
+  return env->NewObject(rotation3dCls, constructor, q.obj());
+}
+
+static jobject MakeJObject(JNIEnv* env, const Transform3d& xform) {
+  static jmethodID constructor =
+      env->GetMethodID(transform3dCls, "<init>",
+                       "(Ledu/wpi/first/math/geometry/Translation3d;"
+                       "Ledu/wpi/first/math/geometry/Rotation3d;)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  JLocal<jobject> xlate{env, MakeJObject(env, xform.Translation())};
+  JLocal<jobject> rot{env, MakeJObject(env, xform.Rotation())};
+  return env->NewObject(transform3dCls, constructor, xlate.obj(), rot.obj());
+}
+
+static jobject MakeJObject(JNIEnv* env, const AprilTagPoseEstimate& est) {
+  static jmethodID constructor =
+      env->GetMethodID(poseEstimateCls, "<init>",
+                       "(Ledu/wpi/first/math/geometry/Transform3d;"
+                       "Ledu/wpi/first/math/geometry/Transform3d;DD)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  JLocal<jobject> pose1{env, MakeJObject(env, est.pose1)};
+  JLocal<jobject> pose2{env, MakeJObject(env, est.pose2)};
+  return env->NewObject(poseEstimateCls, constructor, pose1.obj(), pose2.obj(),
+                        static_cast<jdouble>(est.error1),
+                        static_cast<jdouble>(est.error2));
+}
+
+extern "C" {
+
 /*
  * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
- * Method:    aprilTagDestroy
+ * Method:    createDetector
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_createDetector
+  (JNIEnv* env, jclass)
+{
+  return reinterpret_cast<jlong>(new AprilTagDetector);
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    destroyDetector
  * Signature: (J)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagDestroy
-  (JNIEnv* env, jclass clazz, jlong detectIdx)
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_destroyDetector
+  (JNIEnv* env, jclass, jlong det)
 {
-  auto state =
-      std::find_if(detectors.begin(), detectors.end(),
-                   [&](DetectorState& s) { return s.id == detectIdx; });
+  delete reinterpret_cast<AprilTagDetector*>(det);
+}
 
-  if (state == detectors.end()) {
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    setDetectorConfig
+ * Signature: (JLjava/lang/Object;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_setDetectorConfig
+  (JNIEnv* env, jclass, jlong det, jobject config)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
     return;
   }
-
-  if (state->td) {
-    apriltag_detector_destroy(state->td);
-    state->td = nullptr;
-  }
-  if (state->tf) {
-    state->tf_destroy(state->tf);
-    state->tf = nullptr;
-  }
-
-  detectors.erase(detectors.begin() + detectIdx);
+  reinterpret_cast<AprilTagDetector*>(det)->SetConfig(
+      FromJavaDetectorConfig(env, config));
 }
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    getDetectorConfig
+ * Signature: (J)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_getDetectorConfig
+  (JNIEnv* env, jclass, jlong det)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return nullptr;
+  }
+  return MakeJObject(env,
+                     reinterpret_cast<AprilTagDetector*>(det)->GetConfig());
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    setDetectorQTP
+ * Signature: (JLjava/lang/Object;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_setDetectorQTP
+  (JNIEnv* env, jclass, jlong det, jobject params)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return;
+  }
+  reinterpret_cast<AprilTagDetector*>(det)->SetQuadThresholdParameters(
+      FromJavaDetectorQTP(env, params));
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    getDetectorQTP
+ * Signature: (J)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_getDetectorQTP
+  (JNIEnv* env, jclass, jlong det)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return nullptr;
+  }
+  return MakeJObject(
+      env,
+      reinterpret_cast<AprilTagDetector*>(det)->GetQuadThresholdParameters());
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    addFamily
+ * Signature: (JLjava/lang/String;I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_addFamily
+  (JNIEnv* env, jclass, jlong det, jstring fam, jint bitsCorrected)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return false;
+  }
+  if (!fam) {
+    nullPointerEx.Throw(env, "fam cannot be null");
+    return false;
+  }
+  return reinterpret_cast<AprilTagDetector*>(det)->AddFamily(
+      JStringRef{env, fam}, bitsCorrected);
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    removeFamily
+ * Signature: (JLjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_removeFamily
+  (JNIEnv* env, jclass, jlong det, jstring fam)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return;
+  }
+  if (!fam) {
+    nullPointerEx.Throw(env, "fam cannot be null");
+    return;
+  }
+  reinterpret_cast<AprilTagDetector*>(det)->RemoveFamily(JStringRef{env, fam});
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    clearFamilies
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_clearFamilies
+  (JNIEnv* env, jclass, jlong det)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return;
+  }
+  reinterpret_cast<AprilTagDetector*>(det)->ClearFamilies();
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    detect
+ * Signature: (JIIIJ)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_detect
+  (JNIEnv* env, jclass, jlong det, jint width, jint height, jint stride,
+   jlong bufAddr)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return nullptr;
+  }
+  if (bufAddr == 0) {
+    nullPointerEx.Throw(env, "bufAddr cannot be null");
+    return nullptr;
+  }
+  return MakeJObject(
+      env, reinterpret_cast<AprilTagDetector*>(det)->Detect(
+               width, height, stride, reinterpret_cast<uint8_t*>(bufAddr)));
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    estimatePoseHomography
+ * Signature: ([DDDDDD)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePoseHomography
+  (JNIEnv* env, jclass, jdoubleArray homography, jdouble tagSize, jdouble fx,
+   jdouble fy, jdouble cx, jdouble cy)
+{
+  if (!homography) {
+    nullPointerEx.Throw(env, "homography cannot be null");
+    return nullptr;
+  }
+  JDoubleArrayRef harr{env, homography};
+  if (harr.size() != 9) {
+    illegalArgEx.Throw(env, "homography array must be size 9");
+    return nullptr;
+  }
+
+  AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy});
+  return MakeJObject(env, estimator.EstimateHomography(
+                              std::span<const double, 9>{harr.array()}));
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    estimatePoseOrthogonalIteration
+ * Signature: ([D[DDDDDDI)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePoseOrthogonalIteration
+  (JNIEnv* env, jclass, jdoubleArray homography, jdoubleArray corners,
+   jdouble tagSize, jdouble fx, jdouble fy, jdouble cx, jdouble cy, jint nIters)
+{
+  // homography
+  if (!homography) {
+    nullPointerEx.Throw(env, "homography cannot be null");
+    return nullptr;
+  }
+  JDoubleArrayRef harr{env, homography};
+  if (harr.size() != 9) {
+    illegalArgEx.Throw(env, "homography array must be size 9");
+    return nullptr;
+  }
+
+  // corners
+  if (!corners) {
+    nullPointerEx.Throw(env, "corners cannot be null");
+    return nullptr;
+  }
+  JDoubleArrayRef carr{env, corners};
+  if (carr.size() != 8) {
+    illegalArgEx.Throw(env, "corners array must be size 8");
+    return nullptr;
+  }
+
+  AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy});
+  return MakeJObject(env,
+                     estimator.EstimateOrthogonalIteration(
+                         std::span<const double, 9>{harr.array()},
+                         std::span<const double, 8>{carr.array()}, nIters));
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    estimatePose
+ * Signature: ([D[DDDDDD)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePose
+  (JNIEnv* env, jclass, jdoubleArray homography, jdoubleArray corners,
+   jdouble tagSize, jdouble fx, jdouble fy, jdouble cx, jdouble cy)
+{
+  // homography
+  if (!homography) {
+    nullPointerEx.Throw(env, "homography cannot be null");
+    return nullptr;
+  }
+  JDoubleArrayRef harr{env, homography};
+  if (harr.size() != 9) {
+    illegalArgEx.Throw(env, "homography array must be size 9");
+    return nullptr;
+  }
+
+  // corners
+  if (!corners) {
+    nullPointerEx.Throw(env, "corners cannot be null");
+    return nullptr;
+  }
+  JDoubleArrayRef carr{env, corners};
+  if (carr.size() != 8) {
+    illegalArgEx.Throw(env, "corners array must be size 8");
+    return nullptr;
+  }
+
+  AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy});
+  return MakeJObject(
+      env, estimator.Estimate(std::span<const double, 9>{harr.array()},
+                              std::span<const double, 8>{carr.array()}));
+}
+
 }  // extern "C"

apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h

@@ -0,0 +1,160 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <span>
+#include <string_view>
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
+
+namespace frc {
+
+/**
+ * A detection of an AprilTag tag.
+ */
+class WPILIB_DLLEXPORT AprilTagDetection final {
+ public:
+  AprilTagDetection() = delete;
+  AprilTagDetection(const AprilTagDetection&) = delete;
+  AprilTagDetection& operator=(const AprilTagDetection&) = delete;
+
+  /** A point. Used for center and corner points. */
+  struct Point {
+    double x;
+    double y;
+  };
+
+  /**
+   * Gets the decoded tag's family name.
+   *
+   * @return Decoded family name
+   */
+  std::string_view GetFamily() const;
+
+  /**
+   * Gets the decoded ID of the tag.
+   *
+   * @return Decoded ID
+   */
+  int GetId() const { return id; }
+
+  /**
+   * Gets how many error bits were corrected. Note: accepting large numbers of
+   * corrected errors leads to greatly increased false positive rates.
+   * NOTE: As of this implementation, the detector cannot detect tags with
+   * a hamming distance greater than 2.
+   *
+   * @return Hamming distance (number of corrected error bits)
+   */
+  int GetHamming() const { return hamming; }
+
+  /**
+   * Gets a measure of the quality of the binary decoding process: the
+   * average difference between the intensity of a data bit versus
+   * the decision threshold. Higher numbers roughly indicate better
+   * decodes. This is a reasonable measure of detection accuracy
+   * only for very small tags-- not effective for larger tags (where
+   * we could have sampled anywhere within a bit cell and still
+   * gotten a good detection.)
+   *
+   * @return Decision margin
+   */
+  float GetDecisionMargin() const { return decision_margin; }
+
+  /**
+   * Gets the 3x3 homography matrix describing the projection from an
+   * "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1,
+   * -1)) to pixels in the image.
+   *
+   * @return Homography matrix data
+   */
+  std::span<const double, 9> GetHomography() const;
+
+  /**
+   * Gets the 3x3 homography matrix describing the projection from an
+   * "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1,
+   * -1)) to pixels in the image.
+   *
+   * @return Homography matrix
+   */
+  Eigen::Matrix3d GetHomographyMatrix() const;
+
+  /**
+   * Gets the center of the detection in image pixel coordinates.
+   *
+   * @return Center point
+   */
+  const Point& GetCenter() const { return *reinterpret_cast<const Point*>(c); }
+
+  /**
+   * Gets a corner of the tag in image pixel coordinates. These always
+   * wrap counter-clock wise around the tag.
+   *
+   * @param ndx Corner index (range is 0-3, inclusive)
+   * @return Corner point
+   */
+  const Point& GetCorner(int ndx) const {
+    return *reinterpret_cast<const Point*>(p[ndx]);
+  }
+
+  /**
+   * Gets the corners of the tag in image pixel coordinates. These always
+   * wrap counter-clock wise around the tag.
+   *
+   * @param cornersBuf Corner point array (X and Y for each corner in order)
+   * @return Corner point array (copy of cornersBuf span)
+   */
+  std::span<double, 8> GetCorners(std::span<double, 8> cornersBuf) const {
+    for (int i = 0; i < 4; i++) {
+      cornersBuf[i * 2] = p[i][0];
+      cornersBuf[i * 2 + 1] = p[i][1];
+    }
+    return cornersBuf;
+  }
+
+ private:
+  // This class *must* be standard-layout-compatible with apriltag_detection
+  // as we use reinterpret_cast from that structure. This means the below
+  // members must exactly match the contents of the apriltag_detection struct.
+
+  // The tag family.
+  void* family;
+
+  // The decoded ID of the tag.
+  int id;
+
+  // How many error bits were corrected? Note: accepting large numbers of
+  // corrected errors leads to greatly increased false positive rates.
+  // NOTE: As of this implementation, the detector cannot detect tags with
+  // a hamming distance greater than 2.
+  int hamming;
+
+  // A measure of the quality of the binary decoding process: the
+  // average difference between the intensity of a data bit versus
+  // the decision threshold. Higher numbers roughly indicate better
+  // decodes. This is a reasonable measure of detection accuracy
+  // only for very small tags-- not effective for larger tags (where
+  // we could have sampled anywhere within a bit cell and still
+  // gotten a good detection.)
+  float decision_margin;
+
+  // The 3x3 homography matrix describing the projection from an
+  // "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1,
+  // -1)) to pixels in the image.
+  void* H;
+
+  // The center of the detection in image pixel coordinates.
+  double c[2];
+
+  // The corners of the tag in image pixel coordinates. These always
+  // wrap counter-clock wise around the tag.
+  double p[4][2];
+};
+
+}  // namespace frc

apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h

@@ -0,0 +1,260 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <memory>
+#include <span>
+#include <string_view>
+#include <utility>
+
+#include <units/angle.h>
+#include <wpi/StringMap.h>
+#include <wpi/SymbolExports.h>
+
+#include "frc/apriltag/AprilTagDetection.h"
+
+namespace frc {
+
+/**
+ * An AprilTag detector engine. This is expensive to set up and tear down, so
+ * most use cases should only create one of these, add a family to it, set up
+ * any other configuration, and repeatedly call Detect().
+ */
+class WPILIB_DLLEXPORT AprilTagDetector {
+ public:
+  /** Detector configuration. */
+  struct Config {
+    bool operator==(const Config&) const = default;
+
+    /**
+     * How many threads should be used for computation. Default is
+     * single-threaded operation (1 thread).
+     */
+    int numThreads = 1;
+
+    /**
+     * Quad decimation. Detection of quads can be done on a lower-resolution
+     * image, improving speed at a cost of pose accuracy and a slight decrease
+     * in detection rate. Decoding the binary payload is still done at full
+     * resolution. Default is 2.0.
+     */
+    float quadDecimate = 2.0f;
+
+    /**
+     * What Gaussian blur should be applied to the segmented image (used for
+     * quad detection). Very noisy images benefit from non-zero values (e.g.
+     * 0.8). Default is 0.0.
+     */
+    float quadSigma = 0.0f;
+
+    /**
+     * When true, the edges of the each quad are adjusted to "snap to" strong
+     * gradients nearby. This is useful when decimation is employed, as it can
+     * increase the quality of the initial quad estimate substantially.
+     * Generally recommended to be on (true). Default is true.
+     *
+     * Very computationally inexpensive. Option is ignored if
+     * quad_decimate = 1.
+     */
+    bool refineEdges = true;
+
+    /**
+     * How much sharpening should be done to decoded images. This can help
+     * decode small tags but may or may not help in odd lighting conditions or
+     * low light conditions. Default is 0.25.
+     */
+    double decodeSharpening = 0.25;
+
+    /**
+     * Debug mode. When true, the decoder writes a variety of debugging images
+     * to the current working directory at various stages through the detection
+     * process. This is slow and should *not* be used on space-limited systems
+     * such as the RoboRIO. Default is disabled (false).
+     */
+    bool debug = false;
+  };
+
+  /** Quad threshold parameters. */
+  struct QuadThresholdParameters {
+    bool operator==(const QuadThresholdParameters&) const = default;
+
+    /**
+     * Threshold used to reject quads containing too few pixels. Default is 5
+     * pixels.
+     */
+    int minClusterPixels = 5;
+
+    /**
+     * How many corner candidates to consider when segmenting a group of pixels
+     * into a quad. Default is 10.
+     */
+    int maxNumMaxima = 10;
+
+    /**
+     * Critical angle. The detector will reject quads where pairs of edges have
+     * angles that are close to straight or close to 180 degrees. Zero means
+     * that no quads are rejected. Default is 10 degrees.
+     */
+    units::radian_t criticalAngle = 10_deg;
+
+    /**
+     * When fitting lines to the contours, the maximum mean squared error
+     * allowed. This is useful in rejecting contours that are far from being
+     * quad shaped; rejecting these quads "early" saves expensive decoding
+     * processing. Default is 10.0.
+     */
+    float maxLineFitMSE = 10.0f;
+
+    /**
+     * Minimum brightness offset. When we build our model of black & white
+     * pixels, we add an extra check that the white model must be (overall)
+     * brighter than the black model. How much brighter? (in pixel values,
+     * [0,255]). Default is 5.
+     */
+    int minWhiteBlackDiff = 5;
+
+    /**
+     * Whether the thresholded image be should be deglitched. Only useful for
+     * very noisy images. Default is disabled (false).
+     */
+    bool deglitch = false;
+  };
+
+  /**
+   * Array of detection results. Each array element is a pointer to an
+   * AprilTagDetection.
+   */
+  class WPILIB_DLLEXPORT Results
+      : public std::span<AprilTagDetection const* const> {
+    struct private_init {};
+    friend class AprilTagDetector;
+
+   public:
+    Results() = default;
+    Results(void* impl, const private_init&);
+    ~Results() { Destroy(); }
+    Results(const Results&) = delete;
+    Results& operator=(const Results&) = delete;
+    Results(Results&& rhs) : span{std::move(rhs)}, m_impl{rhs.m_impl} {
+      rhs.m_impl = nullptr;
+    }
+    Results& operator=(Results&& rhs);
+
+   private:
+    void Destroy();
+    void* m_impl = nullptr;
+  };
+
+  AprilTagDetector();
+  ~AprilTagDetector() { Destroy(); }
+  AprilTagDetector(const AprilTagDetector&) = delete;
+  AprilTagDetector& operator=(const AprilTagDetector&) = delete;
+  AprilTagDetector(AprilTagDetector&& rhs)
+      : m_impl{rhs.m_impl},
+        m_families{std::move(rhs.m_families)},
+        m_qtpCriticalAngle{rhs.m_qtpCriticalAngle} {
+    rhs.m_impl = nullptr;
+  }
+  AprilTagDetector& operator=(AprilTagDetector&& rhs);
+
+  /**
+   * @{
+   * @name Configuration functions
+   */
+
+  /**
+   * Sets detector configuration.
+   *
+   * @param config Configuration
+   */
+  void SetConfig(const Config& config);
+
+  /**
+   * Gets detector configuration.
+   *
+   * @return Configuration
+   */
+  Config GetConfig() const;
+
+  /**
+   * Sets quad threshold parameters.
+   *
+   * @param params Parameters
+   */
+  void SetQuadThresholdParameters(const QuadThresholdParameters& params);
+
+  /**
+   * Gets quad threshold parameters.
+   *
+   * @return Parameters
+   */
+  QuadThresholdParameters GetQuadThresholdParameters() const;
+
+  /** @} */
+
+  /**
+   * @{
+   * @name Tag family functions
+   */
+
+  /**
+   * Adds a family of tags to be detected.
+   *
+   * @param fam Family name, e.g. "tag16h5"
+   * @param bitsCorrected
+   * @return False if family can't be found
+   */
+  bool AddFamily(std::string_view fam, int bitsCorrected = 2);
+
+  /**
+   * Removes a family of tags from the detector.
+   *
+   * @param fam Family name, e.g. "tag16h5"
+   */
+  void RemoveFamily(std::string_view fam);
+
+  /**
+   * Unregister all families.
+   */
+  void ClearFamilies();
+
+  /** @} */
+
+  /**
+   * Detect tags from an 8-bit image.
+   *
+   * @param width width of the image
+   * @param height height of the image
+   * @param stride number of bytes between image rows (often the same as width)
+   * @param buf image buffer
+   * @return Results (array of AprilTagDetection pointers)
+   */
+  Results Detect(int width, int height, int stride, uint8_t* buf);
+
+  /**
+   * Detect tags from an 8-bit image.
+   *
+   * @param width width of the image
+   * @param height height of the image
+   * @param buf image buffer
+   * @return Results (array of AprilTagDetection pointers)
+   */
+  Results Detect(int width, int height, uint8_t* buf) {
+    return Detect(width, height, width, buf);
+  }
+
+ private:
+  void Destroy();
+  void DestroyFamilies();
+  void DestroyFamily(std::string_view name, void* data);
+
+  void* m_impl;
+  wpi::StringMap<void*> m_families;
+  units::radian_t m_qtpCriticalAngle = 10_deg;
+};
+
+}  // namespace frc

apriltag/src/main/native/include/frc/apriltag/AprilTagDetector_cv.h

@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <opencv2/core/mat.hpp>
+
+#include "frc/apriltag/AprilTagDetector.h"
+
+namespace frc {
+
+inline AprilTagDetector::Results AprilTagDetect(AprilTagDetector& detector,
+                                                cv::Mat& image) {
+  return detector.Detect(image.cols, image.rows, image.data);
+}
+
+}  // namespace frc

apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h

@@ -31,13 +31,14 @@ namespace frc {
  * "width" and "length" values.  This is to account for arbitrary field sizes
  * when transforming the poses.
  *
- * Pose3ds are assumed to be measured from the bottom-left corner of the field,
- * when the blue alliance is at the left. By default, Pose3ds will be returned
- * as declared when calling GetTagPose(int).
- * SetOrigin(AprilTagFieldLayout::OriginPosition) can be used to transform the
- * poses returned by GetTagPose(int) to be correct relative to a different
- * coordinate frame.
- */
+ * Pose3ds in the JSON are measured using the normal FRC coordinate system, NWU
+ * with the origin at the bottom-right corner of the blue alliance wall.
+ * SetOrigin(OriginPosition) can be used to change the poses returned from
+ * GetTagPose(int) to be from the perspective of a specific alliance.
+ *
+ * Tag poses represent the center of the tag, with a zero rotation representing
+ * a tag that is upright and facing away from the (blue) alliance wall (that is,
+ * towards the opposing alliance). */
 class WPILIB_DLLEXPORT AprilTagFieldLayout {
  public:
   enum class OriginPosition {

apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h

@@ -14,6 +14,7 @@ namespace frc {
 
 enum class AprilTagField {
   k2022RapidReact,
+  k2023ChargedUp,
 
   // This is a placeholder for denoting the last supported field. This should
   // always be the last entry in the enum and should not be used by users

apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimate.h

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Transform3d.h"
+
+namespace frc {
+
+/** A pair of AprilTag pose estimates. */
+struct WPILIB_DLLEXPORT AprilTagPoseEstimate {
+  /** Pose 1. */
+  Transform3d pose1;
+
+  /** Pose 2. */
+  Transform3d pose2;
+
+  /** Object-space error of pose 1. */
+  double error1;
+
+  /** Object-space error of pose 2. */
+  double error2;
+
+  /**
+   * Gets the ratio of pose reprojection errors, called ambiguity. Numbers
+   * above 0.2 are likely to be ambiguous.
+   *
+   * @return The ratio of pose reprojection errors.
+   */
+  double GetAmbiguity() const;
+};
+
+}  // namespace frc

apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimator.h

@@ -0,0 +1,145 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <span>
+
+#include <units/length.h>
+#include <wpi/SymbolExports.h>
+
+#include "frc/apriltag/AprilTagPoseEstimate.h"
+#include "frc/geometry/Transform3d.h"
+
+namespace frc {
+
+class AprilTagDetection;
+
+/** Pose estimators for AprilTag tags. */
+class WPILIB_DLLEXPORT AprilTagPoseEstimator {
+ public:
+  /** Configuration for the pose estimator. */
+  struct Config {
+    bool operator==(const Config&) const = default;
+
+    /** The tag size. */
+    units::meter_t tagSize;
+
+    /** Camera horizontal focal length, in pixels. */
+    double fx;
+
+    /** Camera vertical focal length, in pixels. */
+    double fy;
+
+    /** Camera horizontal focal center, in pixels. */
+    double cx;
+
+    /** Camera vertical focal center, in pixels. */
+    double cy;
+  };
+
+  /**
+   * Creates estimator.
+   *
+   * @param config Configuration
+   */
+  explicit AprilTagPoseEstimator(const Config& config) : m_config{config} {}
+
+  /**
+   * Sets estimator configuration.
+   *
+   * @param config Configuration
+   */
+  void SetConfig(const Config& config) { m_config = config; }
+
+  /**
+   * Gets estimator configuration.
+   *
+   * @return Configuration
+   */
+  const Config& GetConfig() const { return m_config; }
+
+  /**
+   * Estimates the pose of the tag using the homography method described in [1].
+   *
+   * @param detection Tag detection
+   * @return Pose estimate
+   */
+  Transform3d EstimateHomography(const AprilTagDetection& detection) const;
+
+  /**
+   * Estimates the pose of the tag using the homography method described in [1].
+   *
+   * @param homography Homography 3x3 matrix data
+   * @return Pose estimate
+   */
+  Transform3d EstimateHomography(std::span<const double, 9> homography) const;
+
+  /**
+   * Estimates the pose of the tag. This returns one or two possible poses for
+   * the tag, along with the object-space error of each.
+   *
+   * This uses the homography method described in [1] for the initial estimate.
+   * Then Orthogonal Iteration [2] is used to refine this estimate. Then [3] is
+   * used to find a potential second local minima and Orthogonal Iteration is
+   * used to refine this second estimate.
+   *
+   * [1]: E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in
+   *      2011 IEEE International Conference on Robotics and Automation,
+   *      May 2011, pp. 3400–3407.
+   * [2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose
+   *      estimation from video images," in IEEE Transactions on Pattern
+   * Analysis and Machine Intelligence, vol. 22, no. 6, pp. 610-622, June 2000.
+   *      doi: 10.1109/34.862199
+   * [3]: Schweighofer and A. Pinz, "Robust Pose Estimation from a Planar
+   * Target," in IEEE Transactions on Pattern Analysis and Machine Intelligence,
+   *      vol. 28, no. 12, pp. 2024-2030, Dec. 2006. doi: 10.1109/TPAMI.2006.252
+   *
+   * @param detection Tag detection
+   * @param nIters Number of iterations
+   * @return Initial and (possibly) second pose estimates
+   */
+  AprilTagPoseEstimate EstimateOrthogonalIteration(
+      const AprilTagDetection& detection, int nIters) const;
+
+  /**
+   * Estimates the pose of the tag. This returns one or two possible poses for
+   * the tag, along with the object-space error of each.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @param nIters Number of iterations
+   * @return Initial and (possibly) second pose estimates
+   */
+  AprilTagPoseEstimate EstimateOrthogonalIteration(
+      std::span<const double, 9> homography, std::span<const double, 8> corners,
+      int nIters) const;
+
+  /**
+   * Estimates tag pose. This method is an easier to use interface to
+   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the
+   * pose with the lower object-space error.
+   *
+   * @param detection Tag detection
+   * @return Pose estimate
+   */
+  Transform3d Estimate(const AprilTagDetection& detection) const;
+
+  /**
+   * Estimates tag pose. This method is an easier to use interface to
+   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the
+   * pose with the lower object-space error.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @return Pose estimate
+   */
+  Transform3d Estimate(std::span<const double, 9> homography,
+                       std::span<const double, 8> corners) const;
+
+ private:
+  Config m_config;
+};
+
+}  // namespace frc

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2023-chargedup.json

@@ -0,0 +1,152 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 15.513558,
+          "y": 1.071626,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 15.513558,
+          "y": 2.748026,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 15.513558,
+          "y": 4.424426,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 16.178784,
+          "y": 6.749796,
+          "z": 0.695452
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 0.36195,
+          "y": 6.749796,
+          "z": 0.695452
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 1.02743,
+          "y": 4.424426,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 1.02743,
+          "y": 2.748026,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 1.02743,
+          "y": 1.071626,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.54175,
+    "width": 8.0137
+  }
+}

apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java

@@ -0,0 +1,264 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.apriltag;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.fail;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.ByteArrayOutputStream;
+import java.io.IOException;
+import java.io.InputStream;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.opencv.core.Core;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.Imgproc;
+
+@SuppressWarnings("PMD.MutableStaticState")
+class AprilTagDetectorTest {
+  @SuppressWarnings("MemberName")
+  AprilTagDetector detector;
+
+  static RuntimeLoader<Core> loader;
+
+  @BeforeAll
+  static void beforeAll() {
+    try {
+      loader =
+          new RuntimeLoader<>(
+              Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
+      loader.loadLibrary();
+    } catch (IOException ex) {
+      fail(ex);
+    }
+  }
+
+  @BeforeEach
+  void beforeEach() {
+    detector = new AprilTagDetector();
+  }
+
+  @AfterEach
+  void afterEach() {
+    detector.close();
+  }
+
+  @Test
+  void testConfigDefaults() {
+    var config = detector.getConfig();
+    assertEquals(new AprilTagDetector.Config(), config);
+  }
+
+  @Test
+  void testQtpDefaults() {
+    var params = detector.getQuadThresholdParameters();
+    assertEquals(new AprilTagDetector.QuadThresholdParameters(), params);
+  }
+
+  @Test
+  void testSetConfigNumThreads() {
+    var newConfig = new AprilTagDetector.Config();
+    newConfig.numThreads = 2;
+    detector.setConfig(newConfig);
+    var config = detector.getConfig();
+    assertEquals(2, config.numThreads);
+  }
+
+  @Test
+  void testQtpMinClusterPixels() {
+    var newParams = new AprilTagDetector.QuadThresholdParameters();
+    newParams.minClusterPixels = 8;
+    detector.setQuadThresholdParameters(newParams);
+    var params = detector.getQuadThresholdParameters();
+    assertEquals(8, params.minClusterPixels);
+  }
+
+  @Test
+  void testAdd16h5() {
+    assertDoesNotThrow(() -> detector.addFamily("tag16h5"));
+    // duplicate addition is also okay
+    assertDoesNotThrow(() -> detector.addFamily("tag16h5"));
+  }
+
+  @Test
+  void testAdd25h9() {
+    assertDoesNotThrow(() -> detector.addFamily("tag25h9"));
+  }
+
+  @Test
+  void testAdd36h11() {
+    assertDoesNotThrow(() -> detector.addFamily("tag36h11"));
+  }
+
+  @Test
+  void testAddMultiple() {
+    assertDoesNotThrow(() -> detector.addFamily("tag16h5"));
+    assertDoesNotThrow(() -> detector.addFamily("tag36h11"));
+  }
+
+  @Test
+  void testRemoveFamily() {
+    // okay to remove non-existent family
+    detector.removeFamily("tag16h5");
+
+    // add and remove
+    detector.addFamily("tag16h5");
+    detector.removeFamily("tag16h5");
+  }
+
+  @SuppressWarnings("PMD.AssignmentInOperand")
+  public Mat loadImage(String resource) throws IOException {
+    Mat encoded;
+    try (InputStream is = getClass().getResource(resource).openStream()) {
+      try (ByteArrayOutputStream os = new ByteArrayOutputStream(is.available())) {
+        byte[] buffer = new byte[4096];
+        int bytesRead;
+        while ((bytesRead = is.read(buffer)) != -1) {
+          os.write(buffer, 0, bytesRead);
+        }
+        encoded = new Mat(1, os.size(), CvType.CV_8U);
+        encoded.put(0, 0, os.toByteArray());
+      }
+    }
+    Mat image = Imgcodecs.imdecode(encoded, Imgcodecs.IMREAD_COLOR);
+    encoded.release();
+    Imgproc.cvtColor(image, image, Imgproc.COLOR_BGR2GRAY);
+    return image;
+  }
+
+  @Test
+  void testDecodeAndPose() {
+    detector.addFamily("tag16h5");
+    detector.addFamily("tag36h11");
+
+    Mat image;
+    try {
+      image = loadImage("tag1_640_480.jpg");
+    } catch (IOException ex) {
+      fail(ex);
+      return;
+    }
+    try {
+      AprilTagDetection[] results = detector.detect(image);
+      assertEquals(1, results.length);
+      assertEquals("tag36h11", results[0].getFamily());
+      assertEquals(1, results[0].getId());
+      assertEquals(0, results[0].getHamming());
+
+      var estimator =
+          new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0.2, 500, 500, 320, 240));
+      AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 50);
+      assertEquals(new Transform3d(), est.pose2);
+      Transform3d pose = estimator.estimate(results[0]);
+      assertEquals(est.pose1, pose);
+    } finally {
+      image.release();
+    }
+  }
+
+  /**
+   * This tag is rotated such that the top is closer to the camera than the bottom. In the camera
+   * frame, with +x to the right, this is a rotation about +X by 45 degrees.
+   */
+  @Test
+  void testPoseRotatedX() {
+    detector.addFamily("tag16h5");
+
+    Mat image;
+    try {
+      image = loadImage("tag2_45deg_X.png");
+    } catch (IOException ex) {
+      fail(ex);
+      return;
+    }
+    try {
+      AprilTagDetection[] results = detector.detect(image);
+      assertEquals(1, results.length);
+
+      var estimator =
+          new AprilTagPoseEstimator(
+              new AprilTagPoseEstimator.Config(
+                  0.2, 500, 500, image.cols() / 2.0, image.rows() / 2.0));
+      AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 50);
+
+      assertEquals(Units.degreesToRadians(45), est.pose1.getRotation().getX(), 0.1);
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getY(), 0.1);
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getZ(), 0.1);
+    } finally {
+      image.release();
+    }
+  }
+
+  /**
+   * This tag is rotated such that the right is closer to the camera than the left. In the camera
+   * frame, with +y down, this is a rotation of 45 degrees about +y.
+   */
+  @Test
+  void testPoseRotatedY() {
+    detector.addFamily("tag16h5");
+
+    Mat image;
+    try {
+      image = loadImage("tag2_45deg_y.png");
+    } catch (IOException ex) {
+      fail(ex);
+      return;
+    }
+    try {
+      AprilTagDetection[] results = detector.detect(image);
+      assertEquals(1, results.length);
+
+      var estimator =
+          new AprilTagPoseEstimator(
+              new AprilTagPoseEstimator.Config(
+                  0.2, 500, 500, image.cols() / 2.0, image.rows() / 2.0));
+      AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 50);
+
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getX(), 0.1);
+      assertEquals(Units.degreesToRadians(45), est.pose1.getRotation().getY(), 0.1);
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getZ(), 0.1);
+    } finally {
+      image.release();
+    }
+  }
+
+  /** This tag is facing right at the camera -- no rotation should be observed. */
+  @Test
+  void testPoseStraightOn() {
+    detector.addFamily("tag16h5");
+
+    Mat image;
+    try {
+      image = loadImage("tag2_16h5_straight.png");
+    } catch (IOException ex) {
+      fail(ex);
+      return;
+    }
+    try {
+      AprilTagDetection[] results = detector.detect(image);
+      assertEquals(1, results.length);
+
+      var estimator =
+          new AprilTagPoseEstimator(
+              new AprilTagPoseEstimator.Config(
+                  0.2, 500, 500, image.cols() / 2.0, image.rows() / 2.0));
+      AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 50);
+
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getX(), 0.1);
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getY(), 0.1);
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getZ(), 0.1);
+    } finally {
+      image.release();
+    }
+  }
+}

apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp

@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/apriltag/AprilTagDetector.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(AprilTagDetectorTest, ConfigDefaults) {
+  AprilTagDetector detector;
+  auto config = detector.GetConfig();
+  ASSERT_EQ(config, AprilTagDetector::Config{});
+}
+
+TEST(AprilTagDetectorTest, QtpDefaults) {
+  AprilTagDetector detector;
+  auto params = detector.GetQuadThresholdParameters();
+  ASSERT_EQ(params, AprilTagDetector::QuadThresholdParameters{});
+}
+
+TEST(AprilTagDetectorTest, SetConfigNumThreads) {
+  AprilTagDetector detector;
+  detector.SetConfig({.numThreads = 2});
+  auto config = detector.GetConfig();
+  ASSERT_EQ(config.numThreads, 2);
+}
+
+TEST(AprilTagDetectorTest, QtpMinClusterPixels) {
+  AprilTagDetector detector;
+  detector.SetQuadThresholdParameters({.minClusterPixels = 8});
+  auto params = detector.GetQuadThresholdParameters();
+  ASSERT_EQ(params.minClusterPixels, 8);
+}
+
+TEST(AprilTagDetectorTest, Add16h5) {
+  AprilTagDetector detector;
+  ASSERT_TRUE(detector.AddFamily("tag16h5"));
+  // duplicate addition is also okay
+  ASSERT_TRUE(detector.AddFamily("tag16h5"));
+}
+
+TEST(AprilTagDetectorTest, Add25h9) {
+  AprilTagDetector detector;
+  ASSERT_TRUE(detector.AddFamily("tag25h9"));
+}
+
+TEST(AprilTagDetectorTest, Add36h11) {
+  AprilTagDetector detector;
+  ASSERT_TRUE(detector.AddFamily("tag36h11"));
+}
+
+TEST(AprilTagDetectorTest, AddMultiple) {
+  AprilTagDetector detector;
+  ASSERT_TRUE(detector.AddFamily("tag16h5"));
+  ASSERT_TRUE(detector.AddFamily("tag36h11"));
+}
+
+TEST(AprilTagDetectorTest, RemoveFamily) {
+  AprilTagDetector detector;
+  // okay to remove non-existent family
+  detector.RemoveFamily("tag16h5");
+
+  // add and remove
+  detector.AddFamily("tag16h5");
+  detector.RemoveFamily("tag16h5");
+}

build.gradle

@@ -22,7 +22,7 @@ plugins {
     id 'visual-studio'
     id 'net.ltgt.errorprone' version '2.0.2' apply false
     id 'com.github.johnrengelman.shadow' version '7.1.2' apply false
-    id 'com.diffplug.spotless' version '6.4.2' apply false
+    id 'com.diffplug.spotless' version '6.12.0' apply false
     id 'com.github.spotbugs' version '5.0.8' apply false
 }
 

buildSrc/build.gradle

@@ -9,5 +9,5 @@ repositories {
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2023.9.0"
+    implementation "edu.wpi.first:native-utils:2023.11.1"
 }

cscore/.styleguide

@@ -13,6 +13,10 @@ cppSrcFileInclude {
   \.cpp$
 }
 
+modifiableFileExclude {
+  objcpp
+}
+
 licenseUpdateExclude {
   src/main/native/cpp/default_init_allocator\.h$
 }

cscore/CMakeLists.txt

@@ -11,6 +11,7 @@ file(GLOB
     cscore_native_src src/main/native/cpp/*.cpp)
 file(GLOB cscore_linux_src src/main/native/linux/*.cpp)
 file(GLOB cscore_osx_src src/main/native/osx/*.cpp)
+file(GLOB cscore_osx_objc_src src/main/native/objcpp/*.mm)
 file(GLOB cscore_windows_src src/main/native/windows/*.cpp)
 
 add_library(cscore ${cscore_native_src})
@@ -18,7 +19,9 @@ set_target_properties(cscore PROPERTIES DEBUG_POSTFIX "d")
 
 if(NOT MSVC)
     if (APPLE)
-        target_sources(cscore PRIVATE ${cscore_osx_src})
+        target_sources(cscore PRIVATE ${cscore_osx_src} ${cscore_osx_objc_src})
+        target_compile_options(cscore PRIVATE "-fobjc-arc")
+        set_target_properties(cscore PROPERTIES LINK_FLAGS "-framework CoreFoundation -framework AVFoundation -framework Foundation -framework CoreMedia -framework CoreVideo")
     else()
         target_sources(cscore PRIVATE ${cscore_linux_src})
     endif()

cscore/build.gradle

@@ -2,16 +2,16 @@ import org.gradle.internal.os.OperatingSystem
 
 ext {
     nativeName = 'cscore'
-    devMain = 'edu.wpi.cscore.DevMain'
+    devMain = 'edu.wpi.first.cscore.DevMain'
 }
 
 // Removed because having the objective-cpp plugin added breaks
 // embedded tools and its toolchain check. It causes an obj-cpp
 // source set to be added to all binaries, even cross binaries
 // with no support.
-// if (OperatingSystem.current().isMacOsX()) {
-//     apply plugin: 'objective-cpp'
-// }
+if (OperatingSystem.current().isMacOsX()) {
+    apply plugin: 'objective-cpp'
+}
 
 apply from: "${rootDir}/shared/jni/setupBuild.gradle"
 
@@ -87,16 +87,16 @@ ext {
     splitSetup = {
         if (it.targetPlatform.operatingSystem.isMacOsX()) {
             it.sources {
-                // macObjCpp(ObjectiveCppSourceSet) {
-                //     source {
-                //         srcDirs = ['src/main/native/objcpp']
-                //         include '**/*.mm'
-                //     }
-                //     exportedHeaders {
-                //         srcDirs 'src/main/native/include'
-                //         include '**/*.h'
-                //     }
-                // }
+                macObjCpp(ObjectiveCppSourceSet) {
+                    source {
+                        srcDirs = ['src/main/native/objcpp']
+                        include '**/*.mm'
+                    }
+                    exportedHeaders {
+                        srcDirs 'src/main/native/include', 'src/main/native/cpp'
+                        include '**/*.h'
+                    }
+                }
                 cscoreMacCpp(CppSourceSet) {
                     source {
                         srcDirs 'src/main/native/osx'
@@ -157,6 +157,12 @@ Action<List<String>> symbolFilter = { symbols ->
     symbols.removeIf({ !it.startsWith('CS_') })
 } as Action<List<String>>;
 
+run {
+    if (OperatingSystem.current().isMacOsX()) {
+        jvmArgs("-XstartOnFirstThread");
+    }
+}
+
 nativeUtils.exportsConfigs {
     cscore {
         x64ExcludeSymbols = [

cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java

@@ -390,4 +390,10 @@ public class CameraServerJNI {
   public static native long allocateRawFrame();
 
   public static native void freeRawFrame(long frame);
+
+  public static native void runMainRunLoop();
+
+  public static native int runMainRunLoopTimeout(double timeoutSeconds);
+
+  public static native void stopMainRunLoop();
 }

cscore/src/main/native/cpp/SourceImpl.h

@@ -50,6 +50,7 @@ class SourceImpl : public PropertyContainer {
 
   void SetConnectionStrategy(CS_ConnectionStrategy strategy) {
     m_strategy = static_cast<int>(strategy);
+    NumSinksChanged();
   }
   bool IsEnabled() const {
     return m_strategy == CS_CONNECTION_KEEP_OPEN ||

cscore/src/main/native/cpp/UnlimitedHandleResource.h

@@ -154,7 +154,7 @@ template <typename T>
 inline std::span<T>
 UnlimitedHandleResource<THandle, TStruct, typeValue, TMutex>::GetAll(
     wpi::SmallVectorImpl<T>& vec) {
-  ForEach([&](THandle handle, const TStruct& data) { vec.push_back(handle); });
+  ForEach([&](THandle handle, const TStruct&) { vec.push_back(handle); });
   return vec;
 }
 

cscore/src/main/native/cpp/jni/CameraServerJNI.cpp

@@ -13,6 +13,7 @@
 #include "cscore_cpp.h"
 #include "cscore_cv.h"
 #include "cscore_raw.h"
+#include "cscore_runloop.h"
 #include "edu_wpi_first_cscore_CameraServerJNI.h"
 
 namespace cv {
@@ -2226,4 +2227,40 @@ Java_edu_wpi_first_cscore_CameraServerJNI_freeRawFrame
   delete ptr;
 }
 
+/*
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
+ * Method:    runMainRunLoop
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_cscore_CameraServerJNI_runMainRunLoop
+  (JNIEnv*, jclass)
+{
+  cs::RunMainRunLoop();
+}
+
+/*
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
+ * Method:    runMainRunLoopTimeout
+ * Signature: (D)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_cscore_CameraServerJNI_runMainRunLoopTimeout
+  (JNIEnv*, jclass, jdouble timeoutSeconds)
+{
+  return cs::RunMainRunLoopTimeout(timeoutSeconds);
+}
+
+/*
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
+ * Method:    stopMainRunLoop
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_cscore_CameraServerJNI_stopMainRunLoop
+  (JNIEnv*, jclass)
+{
+  return cs::StopMainRunLoop();
+}
+
 }  // extern "C"

cscore/src/main/native/include/cscore_cpp.h

@@ -90,6 +90,11 @@ struct VideoMode : public CS_VideoMode {
     return pixelFormat == other.pixelFormat && width == other.width &&
            height == other.height && fps == other.fps;
   }
+
+  bool CompareWithoutFps(const VideoMode& other) const {
+    return pixelFormat == other.pixelFormat && width == other.width &&
+           height == other.height;
+  }
 };
 
 /**

cscore/src/main/native/include/cscore_runloop.h

@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace cs {
+void RunMainRunLoop();
+int RunMainRunLoopTimeout(double timeoutSeconds);
+void StopMainRunLoop();
+}  // namespace cs

cscore/src/main/native/linux/RunLoopHelpers.cpp

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <wpi/Synchronization.h>
+
+#include "cscore_runloop.h"
+
+static wpi::Event& GetInstance() {
+  static wpi::Event event;
+  return event;
+}
+
+namespace cs {
+void RunMainRunLoop() {
+  wpi::Event& event = GetInstance();
+  wpi::WaitForObject(event.GetHandle());
+}
+
+int RunMainRunLoopTimeout(double timeoutSeconds) {
+  wpi::Event& event = GetInstance();
+  bool timedOut = false;
+  bool signaled =
+      wpi::WaitForObject(event.GetHandle(), timeoutSeconds, &timedOut);
+  if (timedOut) {
+    return 3;
+  }
+  if (signaled) {
+    return 2;
+  }
+  return 1;
+}
+
+void StopMainRunLoop() {
+  wpi::Event& event = GetInstance();
+  event.Set();
+}
+}  // namespace cs

cscore/src/main/native/objcpp/RunLoopHelpers.mm

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "cscore_runloop.h"
+
+#include <CoreFoundation/CFRunLoop.h>
+#import <Foundation/Foundation.h>
+
+namespace cs {
+void RunMainRunLoop() {
+  if (CFRunLoopGetMain() != CFRunLoopGetCurrent()) {
+    NSLog(@"This method can only be called from the main thread");
+    return;
+  }
+  CFRunLoopRun();
+}
+
+int RunMainRunLoopTimeout(double timeoutSeconds) {
+  if (CFRunLoopGetMain() != CFRunLoopGetCurrent()) {
+    NSLog(@"This method can only be called from the main thread");
+    return -1;
+  }
+  return CFRunLoopRunInMode(kCFRunLoopDefaultMode, timeoutSeconds, false);
+}
+
+void StopMainRunLoop() {
+  CFRunLoopStop(CFRunLoopGetMain());
+}
+}

cscore/src/main/native/objcpp/UsbCameraDelegate.h

@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#import <AVFoundation/AVFoundation.h>
+#include <memory>
+
+namespace cs {
+class UsbCameraImpl;
+}
+
+@interface UsbCameraDelegate
+    : NSObject <AVCaptureVideoDataOutputSampleBufferDelegate>
+
+@property(nonatomic) std::weak_ptr<cs::UsbCameraImpl> cppImpl;
+
+- (void)captureOutput:(AVCaptureOutput*)captureOutput
+    didOutputSampleBuffer:(CMSampleBufferRef)sampleBuffer
+           fromConnection:(AVCaptureConnection*)connection;
+@end

cscore/src/main/native/objcpp/UsbCameraDelegate.mm

@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#import "UsbCameraDelegate.h"
+#include "UsbCameraImpl.h"
+
+#include <wpi/timestamp.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+@implementation UsbCameraDelegate
+
+- (id)init {
+  self = [super init];
+  return self;
+}
+
+- (void)captureOutput:(AVCaptureOutput*)captureOutput
+    didOutputSampleBuffer:(CMSampleBufferRef)sampleBuffer
+           fromConnection:(AVCaptureConnection*)connection {
+  (void)captureOutput;
+  (void)sampleBuffer;
+  (void)connection;
+
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    return;
+  }
+
+  // Buffer always comes in a 32BGRA
+  auto imageBuffer = CMSampleBufferGetImageBuffer(sampleBuffer);
+
+  CVPixelBufferLockBaseAddress(imageBuffer, 0);
+
+  void* baseaddress = CVPixelBufferGetBaseAddress(imageBuffer);
+
+  size_t width = CVPixelBufferGetWidth(imageBuffer);
+  size_t height = CVPixelBufferGetHeight(imageBuffer);
+  size_t rowBytes = CVPixelBufferGetBytesPerRow(imageBuffer);
+  OSType pixelFormat = CVPixelBufferGetPixelFormatType(imageBuffer);
+
+  if (rowBytes == 0) {
+    CVPixelBufferUnlockBaseAddress(imageBuffer, 0);
+    return;
+  }
+
+  if (pixelFormat != kCVPixelFormatType_32BGRA) {
+    NSLog(@"Unknown Pixel Format %u", pixelFormat);
+    CVPixelBufferUnlockBaseAddress(imageBuffer, 0);
+    return;
+  }
+
+  size_t currSize = width * 3 * height;
+
+  auto tmpMat = cv::Mat(height, width, CV_8UC4, baseaddress, rowBytes);
+  auto image = sharedThis->AllocImage(cs::VideoMode::PixelFormat::kBGR, width,
+                                      height, currSize);
+  cv::cvtColor(tmpMat, image->AsMat(), cv::COLOR_BGRA2BGR);
+
+  CVPixelBufferUnlockBaseAddress(imageBuffer, 0);
+
+  sharedThis->objcPutFrame(std::move(image), wpi::Now());
+}
+
+@end

cscore/src/main/native/objcpp/UsbCameraImpl.h

@@ -0,0 +1,96 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#import <AVFoundation/AVFoundation.h>
+#import "UsbCameraDelegate.h"
+#import "UsbCameraImplObjc.h"
+
+#include <memory>
+#include <string>
+#include <optional>
+
+#include "SourceImpl.h"
+
+namespace cs {
+struct CameraFPSRange {
+  int min;
+  int max;
+
+  bool IsWithinRange(int fps) { return fps >= min && fps <= max; }
+};
+
+struct CameraModeStore {
+  VideoMode mode;
+  AVCaptureDeviceFormat* format;
+  std::vector<CameraFPSRange> fpsRanges;
+};
+
+class UsbCameraImpl : public SourceImpl {
+ public:
+  UsbCameraImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
+                Telemetry& telemetry, std::string_view path);
+  UsbCameraImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
+                Telemetry& telemetry, int deviceId);
+  ~UsbCameraImpl() override;
+
+  void Start() override;
+
+  // Property functions
+  void SetProperty(int property, int value, CS_Status* status) override;
+  void SetStringProperty(int property, std::string_view value,
+                         CS_Status* status) override;
+
+  // Standard common camera properties
+  void SetBrightness(int brightness, CS_Status* status) override;
+  int GetBrightness(CS_Status* status) const override;
+  void SetWhiteBalanceAuto(CS_Status* status) override;
+  void SetWhiteBalanceHoldCurrent(CS_Status* status) override;
+  void SetWhiteBalanceManual(int value, CS_Status* status) override;
+  void SetExposureAuto(CS_Status* status) override;
+  void SetExposureHoldCurrent(CS_Status* status) override;
+  void SetExposureManual(int value, CS_Status* status) override;
+
+  bool SetVideoMode(const VideoMode& mode, CS_Status* status) override;
+  bool SetPixelFormat(VideoMode::PixelFormat pixelFormat,
+                      CS_Status* status) override;
+  bool SetResolution(int width, int height, CS_Status* status) override;
+  bool SetFPS(int fps, CS_Status* status) override;
+
+  void NumSinksChanged() override;
+  void NumSinksEnabledChanged() override;
+
+  cs::Notifier& objcGetNotifier() { return m_notifier; }
+
+  void objcSwapVideoModes(std::vector<VideoMode>& modes) {
+    std::scoped_lock lock(m_mutex);
+    m_videoModes.swap(modes);
+  }
+
+  void objcSetVideoMode(const VideoMode& mode) {
+    std::scoped_lock lock(m_mutex);
+    m_mode = mode;
+  }
+
+  void objcPutFrame(std::unique_ptr<Image> image, Frame::Time time) {
+    PutFrame(std::move(image), time);
+  }
+
+  const VideoMode& objcGetVideoMode() const { return m_mode; }
+
+  std::vector<CameraModeStore>& objcGetPlatformVideoModes() {
+    return m_platformModes;
+  }
+
+  wpi::Logger& objcGetLogger() { return m_logger; }
+
+  UsbCameraImplObjc* cppGetObjc() { return m_objc; }
+
+ private:
+  UsbCameraImplObjc* m_objc;
+  std::vector<CameraModeStore> m_platformModes;
+  VideoMode m_mode;
+};
+}  // namespace cs

cscore/src/main/native/objcpp/UsbCameraImpl.mm

@@ -0,0 +1,203 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#import <Foundation/Foundation.h>
+#import <AVFoundation/AVFoundation.h>
+
+#include <iostream>
+
+#include <vector>
+#include <string>
+#include <wpi/timestamp.h>
+
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#include "Handle.h"
+#include "Log.h"
+#include "Notifier.h"
+#include "Instance.h"
+#include "c_util.h"
+#include "cscore_cpp.h"
+#include "opencv2/imgproc.hpp"
+#include "UsbCameraImpl.h"
+
+namespace cs {
+
+UsbCameraImpl::UsbCameraImpl(std::string_view name, wpi::Logger& logger,
+                             Notifier& notifier, Telemetry& telemetry,
+                             std::string_view path)
+    : SourceImpl{name, logger, notifier, telemetry} {
+  UsbCameraImplObjc* objc = [[UsbCameraImplObjc alloc] init];
+  objc.path = [[NSString alloc] initWithBytes:path.data()
+                                       length:path.size()
+                                     encoding:NSUTF8StringEncoding];
+  m_objc = objc;
+}
+UsbCameraImpl::UsbCameraImpl(std::string_view name, wpi::Logger& logger,
+                             Notifier& notifier, Telemetry& telemetry,
+                             int deviceId)
+    : SourceImpl{name, logger, notifier, telemetry} {
+  UsbCameraImplObjc* objc = [[UsbCameraImplObjc alloc] init];
+  objc.path = nil;
+  objc.deviceId = deviceId;
+  m_objc = objc;
+}
+
+UsbCameraImpl::~UsbCameraImpl() {
+  m_objc = nil;
+}
+
+void UsbCameraImpl::Start() {
+  [m_objc start];
+}
+
+// Property functions
+void UsbCameraImpl::SetProperty(int property, int value, CS_Status* status) {
+  [m_objc setProperty:property withValue:value status:status];
+}
+void UsbCameraImpl::SetStringProperty(int property, std::string_view value,
+                                      CS_Status* status) {
+  [m_objc setStringProperty:property withValue:&value status:status];
+}
+
+// Standard common camera properties
+void UsbCameraImpl::SetBrightness(int brightness, CS_Status* status) {
+  [m_objc setBrightness:brightness status:status];
+}
+int UsbCameraImpl::GetBrightness(CS_Status* status) const {
+  return [m_objc getBrightness:status];
+}
+void UsbCameraImpl::SetWhiteBalanceAuto(CS_Status* status) {
+  [m_objc setWhiteBalanceAuto:status];
+}
+void UsbCameraImpl::SetWhiteBalanceHoldCurrent(CS_Status* status) {
+  [m_objc setWhiteBalanceHoldCurrent:status];
+}
+void UsbCameraImpl::SetWhiteBalanceManual(int value, CS_Status* status) {
+  [m_objc setWhiteBalanceManual:value status:status];
+}
+void UsbCameraImpl::SetExposureAuto(CS_Status* status) {
+  [m_objc setExposureAuto:status];
+}
+void UsbCameraImpl::SetExposureHoldCurrent(CS_Status* status) {
+  [m_objc setExposureHoldCurrent:status];
+}
+void UsbCameraImpl::SetExposureManual(int value, CS_Status* status) {
+  [m_objc setExposureManual:value status:status];
+}
+
+bool UsbCameraImpl::SetVideoMode(const VideoMode& mode, CS_Status* status) {
+  return [m_objc setVideoMode:mode status:status];
+}
+bool UsbCameraImpl::SetPixelFormat(VideoMode::PixelFormat pixelFormat,
+                                   CS_Status* status) {
+  return [m_objc setPixelFormat:pixelFormat status:status];
+}
+bool UsbCameraImpl::SetResolution(int width, int height, CS_Status* status) {
+  return [m_objc setResolutionWidth:width withHeight:height status:status];
+}
+bool UsbCameraImpl::SetFPS(int fps, CS_Status* status) {
+  return [m_objc setFPS:fps status:status];
+}
+
+void UsbCameraImpl::NumSinksChanged() {
+  [m_objc numSinksChanged];
+}
+void UsbCameraImpl::NumSinksEnabledChanged() {
+  [m_objc numSinksEnabledChanged];
+}
+
+CS_Source CreateUsbCameraDev(std::string_view name, int dev,
+                             CS_Status* status) {
+  std::vector<UsbCameraInfo> devices = cs::EnumerateUsbCameras(status);
+  if (static_cast<int>(devices.size()) > dev) {
+    return CreateUsbCameraPath(name, devices[dev].path, status);
+  }
+  auto& inst = Instance::GetInstance();
+  return inst.CreateSource(CS_SOURCE_USB, std::make_shared<UsbCameraImpl>(
+                                              name, inst.logger, inst.notifier,
+                                              inst.telemetry, dev));
+}
+
+CS_Source CreateUsbCameraPath(std::string_view name, std::string_view path,
+                              CS_Status* status) {
+  (void)status;
+  auto& inst = Instance::GetInstance();
+  auto val = std::make_shared<UsbCameraImpl>(name, inst.logger, inst.notifier,
+                                             inst.telemetry, path);
+  val->cppGetObjc().cppImpl = val;
+  return inst.CreateSource(CS_SOURCE_USB, val);
+}
+
+std::vector<UsbCameraInfo> EnumerateUsbCameras(CS_Status* status) {
+  @autoreleasepool {
+    (void)status;
+    std::vector<UsbCameraInfo> retval;
+    NSArray<AVCaptureDeviceType>* deviceTypes = @[
+      AVCaptureDeviceTypeBuiltInWideAngleCamera,
+      AVCaptureDeviceTypeExternalUnknown
+    ];
+    AVCaptureDeviceDiscoverySession* session = [AVCaptureDeviceDiscoverySession
+        discoverySessionWithDeviceTypes:deviceTypes
+                              mediaType:AVMediaTypeVideo
+                               position:AVCaptureDevicePositionUnspecified];
+
+    NSArray* captureDevices = [session devices];
+
+    int count = 0;
+    for (id device in captureDevices) {
+      NSString* name = [device localizedName];
+      NSString* uniqueIdentifier = [(AVCaptureDevice*)device uniqueID];
+      retval.push_back(
+          {count, [uniqueIdentifier UTF8String], [name UTF8String], {}});
+
+      count++;
+    }
+
+    return retval;
+  }
+}
+
+void SetUsbCameraPath(CS_Source source, std::string_view path,
+                      CS_Status* status) {
+  auto data = Instance::GetInstance().GetSource(source);
+  if (!data || data->kind != CS_SOURCE_USB) {
+    *status = CS_INVALID_HANDLE;
+    return;
+  }
+  [static_cast<UsbCameraImpl&>(*data->source).cppGetObjc()
+      setNewCameraPath:&path];
+}
+
+std::string GetUsbCameraPath(CS_Source source, CS_Status* status) {
+  auto data = Instance::GetInstance().GetSource(source);
+  if (!data || data->kind != CS_SOURCE_USB) {
+    *status = CS_INVALID_HANDLE;
+    return std::string{};
+  }
+  std::string ret;
+  [static_cast<UsbCameraImpl&>(*data->source).cppGetObjc()
+      getCurrentCameraPath:&ret];
+  return ret;
+}
+
+UsbCameraInfo GetUsbCameraInfo(CS_Source source, CS_Status* status) {
+  UsbCameraInfo info;
+  auto data = Instance::GetInstance().GetSource(source);
+  if (!data || data->kind != CS_SOURCE_USB) {
+    *status = CS_INVALID_HANDLE;
+    return info;
+  }
+
+  [static_cast<UsbCameraImpl&>(*data->source).cppGetObjc()
+      getCurrentCameraPath:&info.path];
+  [static_cast<UsbCameraImpl&>(*data->source).cppGetObjc()
+      getCameraName:&info.name];
+  info.productId = 0;
+  info.vendorId = 0;
+  // ParseVidAndPid(info.path, &info.productId, &info.vendorId);
+  info.dev = -1;  // We have lost dev information by this point in time.
+  return info;
+}
+
+}  // namespace cs

cscore/src/main/native/objcpp/UsbCameraImplObjc.h

@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#import <AVFoundation/AVFoundation.h>
+#import "UsbCameraDelegate.h"
+#include <memory>
+#include <string_view>
+#include "cscore_cpp.h"
+
+namespace cs {
+class UsbCameraImpl;
+}
+
+@interface UsbCameraImplObjc : NSObject
+
+@property(nonatomic) AVCaptureDeviceFormat* currentFormat;
+@property(nonatomic) int currentFPS;
+@property(nonatomic) std::weak_ptr<cs::UsbCameraImpl> cppImpl;
+@property(nonatomic) dispatch_queue_t sessionQueue;
+@property(nonatomic) NSString* path;
+@property(nonatomic) int deviceId;
+@property(nonatomic) bool propertiesCached;
+@property(nonatomic) bool streaming;
+@property(nonatomic) bool deviceValid;
+@property(nonatomic) bool isAuthorized;
+
+@property(nonatomic) AVCaptureDevice* videoDevice;
+@property(nonatomic) AVCaptureDeviceInput* videoInput;
+@property(nonatomic) UsbCameraDelegate* callback;
+@property(nonatomic) AVCaptureVideoDataOutput* videoOutput;
+@property(nonatomic) AVCaptureSession* session;
+
+- (void)start;
+
+// Property functions
+- (void)setProperty:(int)property
+          withValue:(int)value
+             status:(CS_Status*)status;
+- (void)setStringProperty:(int)property
+                withValue:(std::string_view*)value
+                   status:(CS_Status*)status;
+
+// Standard common camera properties
+- (void)setBrightness:(int)brightness status:(CS_Status*)status;
+- (int)getBrightness:(CS_Status*)status;
+- (void)setWhiteBalanceAuto:(CS_Status*)status;
+- (void)setWhiteBalanceHoldCurrent:(CS_Status*)status;
+- (void)setWhiteBalanceManual:(int)value status:(CS_Status*)status;
+- (void)setExposureAuto:(CS_Status*)status;
+- (void)setExposureHoldCurrent:(CS_Status*)status;
+- (void)setExposureManual:(int)value status:(CS_Status*)status;
+
+- (bool)setVideoMode:(const cs::VideoMode&)mode status:(CS_Status*)status;
+- (bool)setPixelFormat:(cs::VideoMode::PixelFormat)pixelFormat
+                status:(CS_Status*)status;
+- (bool)setResolutionWidth:(int)width
+                withHeight:(int)height
+                    status:(CS_Status*)status;
+- (bool)setFPS:(int)fps status:(CS_Status*)status;
+
+- (void)numSinksChanged;
+- (void)numSinksEnabledChanged;
+
+- (void)getCurrentCameraPath:(std::string*)path;
+- (void)getCameraName:(std::string*)name;
+- (void)setNewCameraPath:(std::string_view*)path;
+
+@end

cscore/src/main/native/objcpp/UsbCameraImplObjc.mm

@@ -0,0 +1,669 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#import "UsbCameraImplObjc.h"
+#include "UsbCameraImpl.h"
+
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#include "Notifier.h"
+#include "Log.h"
+
+template <typename S, typename... Args>
+inline void NamedLog(UsbCameraImplObjc* objc, unsigned int level,
+                     const char* file, unsigned int line, const S& format,
+                     Args&&... args) {
+  auto sharedThis = objc.cppImpl.lock();
+  if (!sharedThis) {
+    return;
+  }
+
+  wpi::Logger& logger = sharedThis->objcGetLogger();
+  std::string_view name = sharedThis->GetName();
+
+  if (logger.HasLogger() && level >= logger.min_level()) {
+    cs::NamedLogV(logger, level, file, line, name, format,
+                  fmt::make_format_args(args...));
+  }
+}
+
+#define OBJCLOG(level, format, ...)         \
+  NamedLog(self, level, __FILE__, __LINE__, \
+           FMT_STRING(format) __VA_OPT__(, ) __VA_ARGS__)
+
+#define OBJCERROR(format, ...) \
+  OBJCLOG(::wpi::WPI_LOG_ERROR, format __VA_OPT__(, ) __VA_ARGS__)
+#define OBJCWARNING(format, ...) \
+  OBJCLOG(::wpi::WPI_LOG_WARNING, format __VA_OPT__(, ) __VA_ARGS__)
+#define OBJCINFO(format, ...) \
+  OBJCLOG(::wpi::WPI_LOG_INFO, format __VA_OPT__(, ) __VA_ARGS__)
+
+#ifdef NDEBUG
+#define OBJCDEBUG(format, ...) \
+  do {                         \
+  } while (0)
+#define OBJCDEBUG1(format, ...) \
+  do {                          \
+  } while (0)
+#define OBJCDEBUG2(format, ...) \
+  do {                          \
+  } while (0)
+#define OBJCDEBUG3(format, ...) \
+  do {                          \
+  } while (0)
+#define OBJCDEBUG4(format, ...) \
+  do {                          \
+  } while (0)
+#else
+#define OBJCDEBUG(format, ...) \
+  OBJCLOG(::wpi::WPI_LOG_DEBUG, format __VA_OPT__(, ) __VA_ARGS__)
+#define OBJCDEBUG1(format, ...) \
+  OBJCLOG(::wpi::WPI_LOG_DEBUG1, format __VA_OPT__(, ) __VA_ARGS__)
+#define OBJCDEBUG2(format, ...) \
+  OBJCLOG(::wpi::WPI_LOG_DEBUG2, format __VA_OPT__(, ) __VA_ARGS__)
+#define OBJCDEBUG3(format, ...) \
+  OBJCLOG(::wpi::WPI_LOG_DEBUG3, format __VA_OPT__(, ) __VA_ARGS__)
+#define OBJCDEBUG4(format, ...) \
+  OBJCLOG(::wpi::WPI_LOG_DEBUG4, format __VA_OPT__(, ) __VA_ARGS__)
+#endif
+
+using namespace cs;
+
+@implementation UsbCameraImplObjc
+
+- (void)start {
+  switch ([AVCaptureDevice authorizationStatusForMediaType:AVMediaTypeVideo]) {
+    case AVAuthorizationStatusAuthorized:
+      self.isAuthorized = true;
+      break;
+    default:
+      OBJCERROR(
+          "Camera access explicitly blocked for application. No cameras are "
+          "accessable");
+      self.isAuthorized = false;
+      // TODO log
+      break;
+    case AVAuthorizationStatusNotDetermined:
+      dispatch_suspend(self.sessionQueue);
+      [AVCaptureDevice requestAccessForMediaType:AVMediaTypeVideo
+                               completionHandler:^(BOOL granted) {
+                                 self.isAuthorized = granted;
+                                 dispatch_resume(self.sessionQueue);
+                               }];
+      break;
+  }
+  dispatch_async(self.sessionQueue, ^{
+    [[NSNotificationCenter defaultCenter]
+        addObserver:self
+           selector:@selector(cameraConnected:)
+               name:AVCaptureDeviceWasConnectedNotification
+             object:nil];
+    [[NSNotificationCenter defaultCenter]
+        addObserver:self
+           selector:@selector(cameraDisconnected:)
+               name:AVCaptureDeviceWasDisconnectedNotification
+             object:nil];
+    [self deviceConnect];
+  });
+}
+
+// Property functions
+- (void)setProperty:(int)property
+          withValue:(int)value
+             status:(CS_Status*)status {
+}
+- (void)setStringProperty:(int)property
+                withValue:(std::string_view*)value
+                   status:(CS_Status*)status {
+}
+
+// Standard common camera properties
+- (void)setBrightness:(int)brightness status:(CS_Status*)status {
+  *status = CS_INVALID_PROPERTY;
+}
+- (int)getBrightness:(CS_Status*)status {
+  *status = CS_INVALID_PROPERTY;
+  return 0;
+}
+- (void)setWhiteBalanceAuto:(CS_Status*)status {
+  *status = CS_INVALID_PROPERTY;
+}
+- (void)setWhiteBalanceHoldCurrent:(CS_Status*)status {
+  *status = CS_INVALID_PROPERTY;
+}
+- (void)setWhiteBalanceManual:(int)value status:(CS_Status*)status {
+  *status = CS_INVALID_PROPERTY;
+}
+- (void)setExposureAuto:(CS_Status*)status {
+  *status = CS_INVALID_PROPERTY;
+}
+- (void)setExposureHoldCurrent:(CS_Status*)status {
+  *status = CS_INVALID_PROPERTY;
+}
+- (void)setExposureManual:(int)value status:(CS_Status*)status {
+  *status = CS_INVALID_PROPERTY;
+}
+
+- (bool)setVideoMode:(const cs::VideoMode&)mode status:(CS_Status*)status {
+  dispatch_async_and_wait(self.sessionQueue, ^{
+    auto sharedThis = self.cppImpl.lock();
+    if (!sharedThis) {
+      *status = CS_READ_FAILED;
+      return;
+    }
+
+    [self internalSetMode:mode status:status];
+  });
+  return true;
+}
+- (bool)setPixelFormat:(cs::VideoMode::PixelFormat)pixelFormat
+                status:(CS_Status*)status {
+  dispatch_async_and_wait(self.sessionQueue, ^{
+    auto sharedThis = self.cppImpl.lock();
+    if (!sharedThis) {
+      *status = CS_READ_FAILED;
+      return;
+    }
+    VideoMode newMode;
+    newMode = sharedThis->objcGetVideoMode();
+    newMode.pixelFormat = pixelFormat;
+
+    [self internalSetMode:newMode status:status];
+  });
+  return true;
+}
+- (bool)setResolutionWidth:(int)width
+                withHeight:(int)height
+                    status:(CS_Status*)status {
+  dispatch_async_and_wait(self.sessionQueue, ^{
+    auto sharedThis = self.cppImpl.lock();
+    if (!sharedThis) {
+      *status = CS_READ_FAILED;
+      return;
+    }
+    VideoMode newMode;
+    newMode = sharedThis->objcGetVideoMode();
+    newMode.width = width;
+    newMode.height = height;
+
+    [self internalSetMode:newMode status:status];
+  });
+  return true;
+}
+
+- (void)internalSetMode:(const cs::VideoMode&)newMode
+                 status:(CS_Status*)status {
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    *status = CS_READ_FAILED;
+    return;
+  }
+  // If device is not connected, just apply and leave.
+  if (!self.propertiesCached) {
+    sharedThis->objcSetVideoMode(newMode);
+    *status = CS_OK;
+    return;
+  }
+
+  if (newMode != sharedThis->objcGetVideoMode()) {
+    OBJCDEBUG3("Trying Mode {} {} {} {}", newMode.pixelFormat, newMode.width,
+               newMode.height, newMode.fps);
+    int localFPS = 0;
+    AVCaptureDeviceFormat* newModeType = [self deviceCheckModeValid:&newMode
+                                                            withFps:&localFPS];
+    if (newModeType == nil) {
+      *status = CS_UNSUPPORTED_MODE;
+      return;
+    }
+
+    self.currentFormat = newModeType;
+    self.currentFPS = localFPS;
+    sharedThis->objcSetVideoMode(newMode);
+    [self deviceDisconnect];
+    [self deviceConnect];
+    sharedThis->objcGetNotifier().NotifySourceVideoMode(*sharedThis, newMode);
+  }
+  *status = CS_OK;
+}
+
+- (bool)setFPS:(int)fps status:(CS_Status*)status {
+  dispatch_async_and_wait(self.sessionQueue, ^{
+    auto sharedThis = self.cppImpl.lock();
+    if (!sharedThis) {
+      *status = CS_READ_FAILED;
+      return;
+    }
+    VideoMode newMode;
+    newMode = sharedThis->objcGetVideoMode();
+    newMode.fps = fps;
+
+    [self internalSetMode:newMode status:status];
+  });
+  return true;
+}
+
+- (void)numSinksChanged {
+  dispatch_async(self.sessionQueue, ^{
+    auto sharedThis = self.cppImpl.lock();
+    if (!sharedThis) {
+      return;
+    }
+    if (!sharedThis->IsEnabled()) {
+      [self deviceStreamOff];
+    } else if (!self.streaming && sharedThis->IsEnabled()) {
+      [self deviceStreamOn];
+    }
+  });
+}
+- (void)numSinksEnabledChanged {
+  [self numSinksChanged];
+}
+
+// All above is direct forwarders from C++, must always dispatch to loop
+
+- (void)getCurrentCameraPath:(std::string*)path {
+  dispatch_async_and_wait(self.sessionQueue, ^{
+    if (self.videoDevice == nil) {
+      return;
+    }
+    *path = [self.videoDevice.uniqueID UTF8String];
+  });
+}
+
+- (void)getCameraName:(std::string*)name {
+  dispatch_async_and_wait(self.sessionQueue, ^{
+    if (self.videoDevice == nil) {
+      return;
+    }
+    *name = [self.videoDevice.localizedName UTF8String];
+  });
+}
+
+- (void)setNewCameraPath:(std::string_view*)path {
+  dispatch_async_and_wait(self.sessionQueue, ^{
+    NSString* nsPath = [[NSString alloc] initWithBytes:path->data()
+                                                length:path->size()
+                                              encoding:NSUTF8StringEncoding];
+    if (self.path != nil && [self.path isEqualToString:nsPath]) {
+      return;
+    }
+    self.path = nsPath;
+    [self deviceDisconnect];
+    [self deviceConnect];
+  });
+}
+
+// All above are called from C++, must always dispatch to loop
+
+- (void)deviceCacheProperties {
+  if (self.session == nil) {
+    return;
+  }
+}
+
+static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) {
+  switch (fourcc) {
+    case kCVPixelFormatType_422YpCbCr8_yuvs:
+    case kCVPixelFormatType_422YpCbCr8FullRange:
+      return cs::VideoMode::PixelFormat::kYUYV;
+    default:
+      return cs::VideoMode::PixelFormat::kBGR;
+  }
+}
+
+- (void)deviceCacheVideoModes {
+  if (self.session == nil) {
+    return;
+  }
+
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    return;
+  }
+  std::vector<CameraModeStore>& platformModes =
+      sharedThis->objcGetPlatformVideoModes();
+  platformModes.clear();
+
+  std::vector<VideoMode> modes;
+  @autoreleasepool {
+    NSArray<AVCaptureDeviceFormat*>* formats = self.videoDevice.formats;
+
+    int count = 0;
+
+    for (AVCaptureDeviceFormat* format in formats) {
+      CMFormatDescriptionRef cmformat = format.formatDescription;
+      CMVideoDimensions s1 = CMVideoFormatDescriptionGetDimensions(cmformat);
+
+      FourCharCode fourcc = CMFormatDescriptionGetMediaSubType(cmformat);
+      auto videoFormat = FourCCToPixelFormat(fourcc);
+
+      NSArray<AVFrameRateRange*>* frameRates =
+          format.videoSupportedFrameRateRanges;
+
+      CameraModeStore store;
+      store.mode.pixelFormat = videoFormat;
+      store.mode.width = static_cast<int>(s1.width);
+      store.mode.height = static_cast<int>(s1.height);
+      store.format = format;
+      int maxFps = 0;
+
+      for (AVFrameRateRange* rate in frameRates) {
+        CMTime highest = rate.minFrameDuration;
+        CMTime lowest = rate.maxFrameDuration;
+
+        int highestFps = highest.timescale / static_cast<double>(highest.value);
+        int lowestFps = lowest.timescale / static_cast<double>(lowest.value);
+
+        store.fpsRanges.emplace_back(CameraFPSRange{lowestFps, highestFps});
+        if (highestFps > maxFps) {
+          maxFps = highestFps;
+        }
+      }
+      store.mode.fps = maxFps;
+
+      modes.emplace_back(store.mode);
+      platformModes.emplace_back(store);
+      count++;
+    }
+  }
+
+  sharedThis->objcSwapVideoModes(modes);
+  sharedThis->objcGetNotifier().NotifySource(*sharedThis,
+                                             CS_SOURCE_VIDEOMODES_UPDATED);
+}
+
+- (AVCaptureDeviceFormat*)deviceCheckModeValid:(const cs::VideoMode*)toCheck
+                                       withFps:(int*)fps {
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    return nil;
+  }
+
+  OBJCDEBUG3("Checking mode {} {} {} {}", toCheck->pixelFormat, toCheck->width,
+             toCheck->height, toCheck->fps);
+  std::vector<CameraModeStore>& platformModes =
+      sharedThis->objcGetPlatformVideoModes();
+  // Find the matching mode
+  auto match = std::find_if(platformModes.begin(), platformModes.end(),
+                            [&](CameraModeStore& input) {
+                              return input.mode.CompareWithoutFps(*toCheck);
+                            });
+
+  if (match == platformModes.end()) {
+    return nil;
+  }
+
+  // Check FPS
+  for (CameraFPSRange& range : match->fpsRanges) {
+    OBJCDEBUG3("Checking Range {} {}", range.min, range.max);
+    if (range.IsWithinRange(toCheck->fps)) {
+      *fps = toCheck->fps;
+      return match->format;
+    }
+  }
+
+  return nil;
+}
+
+- (void)deviceCacheMode {
+  if (!self.session) {
+    return;
+  }
+
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    return;
+  }
+
+  std::vector<CameraModeStore>& platformModes =
+      sharedThis->objcGetPlatformVideoModes();
+
+  if (platformModes.size() == 0) {
+    return;
+  }
+
+  if (self.currentFormat == nil) {
+    int localFps = 0;
+    self.currentFormat =
+        [self deviceCheckModeValid:&sharedThis->objcGetVideoMode()
+                           withFps:&localFps];
+    if (self.currentFormat == nil) {
+      self.currentFormat = self.videoDevice.activeFormat;
+      auto result = std::find_if(platformModes.begin(), platformModes.end(),
+                                 [f = self.currentFormat](CameraModeStore& i) {
+                                   return [f isEqual:i.format];
+                                 });
+      if (result == platformModes.end()) {
+        auto& firstSupported = platformModes[0];
+        self.currentFormat = firstSupported.format;
+        self.currentFPS = firstSupported.mode.fps;
+        sharedThis->objcSetVideoMode(firstSupported.mode);
+      } else {
+        self.currentFPS = result->mode.fps;
+        sharedThis->objcSetVideoMode(result->mode);
+      }
+    } else {
+      self.currentFPS = localFps;
+    }
+  }
+
+  [self deviceSetMode];
+
+  sharedThis->objcGetNotifier().NotifySourceVideoMode(
+      *sharedThis, sharedThis->objcGetVideoMode());
+}
+
+- (void)deviceSetMode {
+  self.deviceValid = true;
+}
+
+- (bool)deviceStreamOn {
+  if (self.streaming) {
+    return false;
+  }
+  if (!self.deviceValid) {
+    return false;
+  }
+  self.streaming = true;
+
+  [self.session startRunning];
+
+  if ([self.videoDevice lockForConfiguration:nil]) {
+    if (self.currentFormat != nil) {
+      self.videoDevice.activeFormat = self.currentFormat;
+    }
+    if (self.currentFPS != 0) {
+      self.videoDevice.activeVideoMinFrameDuration =
+          CMTimeMake(1, self.currentFPS);
+      self.videoDevice.activeVideoMaxFrameDuration =
+          CMTimeMake(1, self.currentFPS);
+    }
+    [self.videoDevice unlockForConfiguration];
+  } else {
+    OBJCERROR("Failed to lock for configuration");
+  }
+
+  return true;
+}
+
+- (bool)deviceStreamOff {
+  if (self.streaming) {
+    [self.session stopRunning];
+  }
+  self.streaming = false;
+  return true;
+}
+
+- (id)init {
+  self = [super init];
+  // TODO pass in name, make this queue specific
+  self.sessionQueue =
+      dispatch_queue_create("session queue", DISPATCH_QUEUE_SERIAL);
+  return self;
+}
+
+- (void)deviceDisconnect {
+  std::string pathStr = [self.path UTF8String];
+  OBJCINFO("Disconnected from {}", pathStr);
+
+  [self deviceStreamOff];
+  self.session = nil;
+  self.videoOutput = nil;
+  self.callback = nil;
+  self.videoInput = nil;
+  self.videoDevice = nil;
+  self.streaming = false;
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    return;
+  }
+  sharedThis->SetConnected(false);
+}
+
+- (bool)deviceConnect {
+  if (!self.isAuthorized) {
+    OBJCERROR(
+        "Camera access not authorized for application. No cameras are "
+        "accessable");
+    return false;
+  }
+
+  OSType pixelFormat = kCVPixelFormatType_32BGRA;
+
+  NSDictionary* pixelBufferOptions =
+      @{(id)kCVPixelBufferPixelFormatTypeKey : @(pixelFormat)};
+
+  if (self.session != nil) {
+    return true;
+  }
+
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    return false;
+  }
+
+  if (self.path == nil) {
+    OBJCINFO("Starting for device id {}", self.deviceId);
+    // Enumerate Devices
+    CS_Status status = 0;
+    auto cameras = cs::EnumerateUsbCameras(&status);
+    if (static_cast<int>(cameras.size()) <= self.deviceId) {
+      return false;
+    }
+    std::string& path = cameras[self.deviceId].path;
+    self.path = [[NSString alloc] initWithBytes:path.data()
+                                         length:path.size()
+                                       encoding:NSUTF8StringEncoding];
+  }
+
+  std::string pathStr = [self.path UTF8String];
+  OBJCINFO("Connecting to USB camera on {}", pathStr);
+
+  self.videoDevice = [AVCaptureDevice deviceWithUniqueID:self.path];
+  if (self.videoDevice == nil) {
+    OBJCWARNING("Device Not found");
+    goto err;
+  }
+
+  self.videoInput = [AVCaptureDeviceInput deviceInputWithDevice:self.videoDevice
+                                                          error:nil];
+  if (self.videoInput == nil) {
+    OBJCWARNING("Creating AVCaptureDeviceInput failed");
+    goto err;
+  }
+
+  self.callback = [[UsbCameraDelegate alloc] init];
+  if (self.callback == nil) {
+    OBJCWARNING("Creating Camera Callback failed");
+    goto err;
+  }
+  self.callback.cppImpl = self.cppImpl;
+
+  self.videoOutput = [[AVCaptureVideoDataOutput alloc] init];
+  if (self.videoOutput == nil) {
+    OBJCWARNING("Creating AVCaptureVideoDataOutput failed");
+    goto err;
+  }
+
+  [self.videoOutput setSampleBufferDelegate:self.callback
+                                      queue:self.sessionQueue];
+
+  self.videoOutput.videoSettings = pixelBufferOptions;
+  self.videoOutput.alwaysDiscardsLateVideoFrames = YES;
+
+  self.session = [[AVCaptureSession alloc] init];
+  if (self.session == nil) {
+    OBJCWARNING("Creating AVCaptureSession failed");
+    goto err;
+  }
+
+  [[NSNotificationCenter defaultCenter]
+      addObserver:self
+         selector:@selector(sessionRuntimeError:)
+             name:AVCaptureSessionRuntimeErrorNotification
+           object:self.session];
+
+  [self.session addInput:self.videoInput];
+  [self.session addOutput:self.videoOutput];
+
+  sharedThis->SetDescription([self.videoDevice.localizedName UTF8String]);
+
+  if (!self.propertiesCached) {
+    OBJCDEBUG3("Caching properties");
+    [self deviceCacheProperties];
+    [self deviceCacheVideoModes];
+    [self deviceCacheMode];
+    self.propertiesCached = true;
+  } else {
+    OBJCDEBUG3("Restoring Video Mode");
+    [self deviceSetMode];
+  }
+
+  sharedThis->SetConnected(true);
+
+  if (sharedThis->IsEnabled()) {
+    [self deviceStreamOn];
+  }
+
+  return true;
+
+err:
+  self.session = nil;
+  self.videoOutput = nil;
+  self.callback = nil;
+  self.videoInput = nil;
+  self.videoDevice = nil;
+  return false;
+}
+
+// Helpers
+- (void)sessionRuntimeError:(NSNotification*)notification {
+  @autoreleasepool {
+    NSError* error = notification.userInfo[AVCaptureSessionErrorKey];
+    const char* str = [error.description UTF8String];
+    if (str) {
+      std::string errorStr = str;
+      OBJCERROR("Capture session runtime error: {}", errorStr);
+    }
+  }
+}
+
+- (void)cameraDisconnected:(NSNotification*)notification {
+  AVCaptureDevice* device = notification.object;
+  dispatch_async(self.sessionQueue, ^{
+    if (self.path != nil && [device.uniqueID isEqualToString:self.path]) {
+      [self deviceDisconnect];
+    }
+  });
+}
+
+- (void)cameraConnected:(NSNotification*)notification {
+  AVCaptureDevice* device = notification.object;
+  dispatch_async(self.sessionQueue, ^{
+    if (self.path == nil || [device.uniqueID isEqualToString:self.path]) {
+      [self deviceConnect];
+    }
+  });
+}
+
+@end

cscore/src/main/native/objcpp/UsbCameraListener.mm

@@ -0,0 +1,111 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#import <AVFoundation/AVFoundation.h>
+#include "UsbCameraListener.h"
+
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#include "Notifier.h"
+
+using namespace cs;
+
+@interface UsbCameraListenerImpl : NSObject
+@property(nonatomic) Notifier* notifier;
+@property BOOL started;
+@property(nonatomic) dispatch_queue_t sessionQueue;
+
+- (void)start;
+- (void)stop;
+
+@end
+
+@implementation UsbCameraListenerImpl
+
+- (id)init {
+  self = [super init];
+  self.sessionQueue =
+      dispatch_queue_create("Camera Listener", DISPATCH_QUEUE_SERIAL);
+  return self;
+}
+
+- (void)start {
+  dispatch_async(self.sessionQueue, ^{
+    if (self.started) {
+      return;
+    }
+    self.started = YES;
+    [[NSNotificationCenter defaultCenter]
+        addObserver:self
+           selector:@selector(camerasChanged:)
+               name:AVCaptureDeviceWasConnectedNotification
+             object:nil];
+    [[NSNotificationCenter defaultCenter]
+        addObserver:self
+           selector:@selector(camerasChanged:)
+               name:AVCaptureDeviceWasDisconnectedNotification
+             object:nil];
+  });
+}
+
+- (void)stop {
+  dispatch_async_and_wait(self.sessionQueue, ^{
+    if (!self.started) {
+      return;
+    }
+    self.started = NO;
+    [[NSNotificationCenter defaultCenter]
+        removeObserver:self
+                  name:AVCaptureDeviceWasConnectedNotification
+                object:nil];
+    [[NSNotificationCenter defaultCenter]
+        removeObserver:self
+                  name:AVCaptureDeviceWasDisconnectedNotification
+                object:nil];
+  });
+}
+
+- (void)camerasChanged:(NSNotification*)notification {
+  @autoreleasepool {
+    dispatch_async(self.sessionQueue, ^{
+      if (!self.started) {
+        return;
+      }
+
+      AVCaptureDevice* device = notification.object;
+      if ([device.deviceType
+              isEqualToString:AVCaptureDeviceTypeBuiltInWideAngleCamera] ||
+          [device.deviceType
+              isEqualToString:AVCaptureDeviceTypeExternalUnknown]) {
+        self.notifier->NotifyUsbCamerasChanged();
+      }
+    });
+  }
+}
+
+@end
+
+class UsbCameraListener::Impl {
+ public:
+  UsbCameraListenerImpl* listener;
+
+  explicit Impl(Notifier& notifier) {
+    listener = [[UsbCameraListenerImpl alloc] init];
+    listener.notifier = &notifier;
+  }
+};
+
+UsbCameraListener::UsbCameraListener(wpi::Logger&, Notifier& notifier)
+    : m_impl{std::make_unique<Impl>(notifier)} {}
+
+UsbCameraListener::~UsbCameraListener() {
+  Stop();
+}
+
+void UsbCameraListener::Start() {
+  [m_impl->listener start];
+}
+
+void UsbCameraListener::Stop() {
+  [m_impl->listener stop];
+}

cscore/src/main/native/objcpp/objcpptemp.mm

@@ -1,12 +0,0 @@
-#import <Foundation/Foundation.h>
-
-@interface XYZPerson : NSObject
-- (void)sayHello;
-@end
-
-
-@implementation XYZPerson
-- (void)sayHello {
-    NSLog(@"Hello, World!");
-}
-@end

cscore/src/main/native/osx/UsbCameraImpl.cpp

@@ -1,48 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include "Instance.h"
-#include "cscore_cpp.h"
-
-namespace cs {
-
-CS_Source CreateUsbCameraDev(std::string_view name, int dev,
-                             CS_Status* status) {
-  *status = CS_INVALID_HANDLE;
-  WPI_ERROR(Instance::GetInstance().logger,
-            "USB Camera support not implemented for macOS");
-  return 0;
-}
-
-CS_Source CreateUsbCameraPath(std::string_view name, std::string_view path,
-                              CS_Status* status) {
-  *status = CS_INVALID_HANDLE;
-  WPI_ERROR(Instance::GetInstance().logger,
-            "USB Camera support not implemented for macOS");
-  return 0;
-}
-
-void SetUsbCameraPath(CS_Source source, std::string_view path,
-                      CS_Status* status) {
-  *status = CS_INVALID_HANDLE;
-}
-
-std::string GetUsbCameraPath(CS_Source source, CS_Status* status) {
-  *status = CS_INVALID_HANDLE;
-  return std::string{};
-}
-
-UsbCameraInfo GetUsbCameraInfo(CS_Source source, CS_Status* status) {
-  *status = CS_INVALID_HANDLE;
-  return UsbCameraInfo{};
-}
-
-std::vector<UsbCameraInfo> EnumerateUsbCameras(CS_Status* status) {
-  *status = CS_INVALID_HANDLE;
-  WPI_ERROR(Instance::GetInstance().logger,
-            "USB Camera support not implemented for macOS");
-  return std::vector<UsbCameraInfo>{};
-}
-
-}  // namespace cs

cscore/src/main/native/osx/UsbCameraListener.cpp

@@ -1,17 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include "UsbCameraListener.h"
-
-using namespace cs;
-
-class UsbCameraListener::Impl {};
-
-UsbCameraListener::UsbCameraListener(wpi::Logger& logger, Notifier& notifier) {}
-
-UsbCameraListener::~UsbCameraListener() = default;
-
-void UsbCameraListener::Start() {}
-
-void UsbCameraListener::Stop() {}

cscore/src/main/native/windows/RunLoopHelpers.cpp

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <wpi/Synchronization.h>
+
+#include "cscore_runloop.h"
+
+static wpi::Event& GetInstance() {
+  static wpi::Event event;
+  return event;
+}
+
+namespace cs {
+void RunMainRunLoop() {
+  wpi::Event& event = GetInstance();
+  wpi::WaitForObject(event.GetHandle());
+}
+
+int RunMainRunLoopTimeout(double timeoutSeconds) {
+  wpi::Event& event = GetInstance();
+  bool timedOut = false;
+  bool signaled =
+      wpi::WaitForObject(event.GetHandle(), timeoutSeconds, &timedOut);
+  if (timedOut) {
+    return 3;
+  }
+  if (signaled) {
+    return 2;
+  }
+  return 1;
+}
+
+void StopMainRunLoop() {
+  wpi::Event& event = GetInstance();
+  event.Set();
+}
+}  // namespace cs

docs/build.gradle

@@ -40,9 +40,16 @@ cppProjectZips.add(project(':wpinet').cppHeadersZip)
 cppProjectZips.add(project(':wpiutil').cppHeadersZip)
 
 doxygen {
-    executables {
-        doxygen version : '1.9.4',
-        baseURI : 'https://frcmaven.wpi.edu/artifactory/generic-release-mirror/doxygen'
+    // Doxygen binaries are only provided for x86_64 platforms
+    // Other platforms will need to provide doxygen via their system
+    // See below maven and https://doxygen.nl/download.html for provided binaries
+
+    String arch = System.getProperty("os.arch");
+    if (arch.equals("x86_64") || arch.equals("amd64")) {
+        executables {
+            doxygen version : '1.9.4',
+            baseURI : 'https://frcmaven.wpi.edu/artifactory/generic-release-mirror/doxygen'
+        }
     }
 }
 

fieldImages/CMakeLists.txt

@@ -11,7 +11,7 @@ if (WITH_JAVA)
     set(CMAKE_JAVA_INCLUDE_PATH fieldImages.jar ${JACKSON_JARS})
 
     file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-    file(GLOB_RECURSE JAVA_RESOURCES src/main/native/resources/*.json src/main/native/resources/*.png src/main/native/resources/*.jpg)
+    file(GLOB_RECURSE JAVA_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} src/main/native/resources/*.json src/main/native/resources/*.png src/main/native/resources/*.jpg)
     add_jar(field_images_jar SOURCES ${JAVA_SOURCES} RESOURCES NAMESPACE "edu/wpi/first/fields" ${JAVA_RESOURCES} OUTPUT_NAME fieldImages)
 
     get_property(FIELD_IMAGES_JAR_FILE TARGET field_images_jar PROPERTY JAR_FILE)

fieldImages/src/main/java/edu/wpi/fields/Fields.java

@@ -14,12 +14,13 @@ public enum Fields {
   k2021GalacticSearchA("2021-galacticsearcha.json"),
   k2021GalacticSearchB("2021-galacticsearchb.json"),
   k2021Slalom("2021-slalompath.json"),
-  k2022RapidReact("2022-rapidreact.json");
+  k2022RapidReact("2022-rapidreact.json"),
+  k2023ChargedUp("2023-chargedup.json");
 
   public static final String kBaseResourceDir = "/edu/wpi/first/fields/";
 
   /** Alias to the current game. */
-  public static final Fields kDefaultField = k2022RapidReact;
+  public static final Fields kDefaultField = k2023ChargedUp;
 
   public final String m_resourceFile;
 

fieldImages/src/main/native/include/fields/2023-chargedup.h

@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+namespace fields {
+std::string_view GetResource_2023_chargedup_json();
+std::string_view GetResource_2023_field_png();
+}  // namespace fields

fieldImages/src/main/native/resources/edu/wpi/first/fields/2023-chargedup.json

@@ -0,0 +1,10 @@
+{
+    "game": "Charged Up",
+    "field-image": "2023-field.png",
+    "field-corners": {
+      "top-left": [46, 36],
+      "bottom-right": [1088, 544]
+    },
+    "field-size": [54.27083, 26.2916],
+    "field-unit": "foot"
+  }

glass/src/lib/native/cpp/Context.cpp

@@ -546,15 +546,24 @@ void glass::PopID() {
 bool glass::PopupEditName(const char* label, std::string* name) {
   bool rv = false;
   if (ImGui::BeginPopupContextItem(label)) {
-    ImGui::Text("Edit name:");
-    if (ImGui::InputText("##editname", name)) {
-      rv = true;
-    }
-    if (ImGui::Button("Close") || ImGui::IsKeyPressedMap(ImGuiKey_Enter) ||
-        ImGui::IsKeyPressedMap(ImGuiKey_KeyPadEnter)) {
-      ImGui::CloseCurrentPopup();
-    }
+    rv = ItemEditName(name);
+
     ImGui::EndPopup();
   }
   return rv;
 }
+
+bool glass::ItemEditName(std::string* name) {
+  bool rv = false;
+
+  ImGui::Text("Edit name:");
+  if (ImGui::InputText("##editname", name)) {
+    rv = true;
+  }
+  if (ImGui::Button("Close") || ImGui::IsKeyPressedMap(ImGuiKey_Enter) ||
+      ImGui::IsKeyPressedMap(ImGuiKey_KeyPadEnter)) {
+    ImGui::CloseCurrentPopup();
+  }
+
+  return rv;
+}

glass/src/lib/native/cpp/View.cpp

@@ -25,3 +25,9 @@ std::unique_ptr<View> glass::MakeFunctionView(
 }
 
 void View::Hidden() {}
+
+void View::Settings() {}
+
+bool View::HasSettings() {
+  return false;
+}

glass/src/lib/native/cpp/Window.cpp

@@ -4,11 +4,13 @@
 
 #include "glass/Window.h"
 
+#include <imgui.h>
 #include <imgui_internal.h>
 #include <wpi/StringExtras.h>
 
 #include "glass/Context.h"
 #include "glass/Storage.h"
+#include "glass/support/ExtraGuiWidgets.h"
 
 using namespace glass;
 
@@ -59,9 +61,57 @@ void Window::Display() {
                 m_id.c_str());
 
   if (Begin(label, &m_visible, m_flags)) {
-    if (m_renamePopupEnabled) {
-      PopupEditName(nullptr, &m_name);
+    if (m_renamePopupEnabled || m_view->HasSettings()) {
+      bool isClicked = (ImGui::IsMouseReleased(ImGuiMouseButton_Right) &&
+                        ImGui::IsItemHovered());
+      ImGuiWindow* window = ImGui::GetCurrentWindow();
+
+      bool settingsButtonClicked = false;
+      // Not docked, and window has just enough for the circles not to be
+      // touching
+      if (!ImGui::IsWindowDocked() &&
+          ImGui::GetWindowWidth() > (ImGui::GetFontSize() + 2) * 3 +
+                                        ImGui::GetStyle().FramePadding.x * 2) {
+        const ImGuiItemFlags itemFlagsRestore =
+            ImGui::GetCurrentContext()->CurrentItemFlags;
+
+        ImGui::GetCurrentContext()->CurrentItemFlags |=
+            ImGuiItemFlags_NoNavDefaultFocus;
+        window->DC.NavLayerCurrent = ImGuiNavLayer_Menu;
+
+        // Allow to draw outside of normal window
+        ImGui::PushClipRect(window->OuterRectClipped.Min,
+                            window->OuterRectClipped.Max, false);
+
+        const ImRect titleBarRect = ImGui::GetCurrentWindow()->TitleBarRect();
+        const ImVec2 position = {titleBarRect.Max.x -
+                                     (ImGui::GetStyle().FramePadding.x * 3) -
+                                     (ImGui::GetFontSize() * 2),
+                                 titleBarRect.Min.y};
+        settingsButtonClicked =
+            HamburgerButton(ImGui::GetID("#SETTINGS"), position);
+
+        ImGui::PopClipRect();
+
+        ImGui::GetCurrentContext()->CurrentItemFlags = itemFlagsRestore;
+      }
+      if (settingsButtonClicked || isClicked) {
+        ImGui::OpenPopup(window->ID);
+      }
+
+      if (ImGui::BeginPopupEx(window->ID,
+                              ImGuiWindowFlags_AlwaysAutoResize |
+                                  ImGuiWindowFlags_NoTitleBar |
+                                  ImGuiWindowFlags_NoSavedSettings)) {
+        if (m_renamePopupEnabled) {
+          ItemEditName(&m_name);
+        }
+        m_view->Settings();
+
+        ImGui::EndPopup();
+      }
     }
+
     m_view->Display();
   } else {
     m_view->Hidden();

glass/src/lib/native/cpp/other/Field2D.cpp

@@ -1217,10 +1217,14 @@ void glass::DisplayField2D(Field2DModel* model, const ImVec2& contentSize) {
 }
 
 void Field2DView::Display() {
-  if (ImGui::BeginPopupContextItem()) {
-    DisplayField2DSettings(m_model);
-    ImGui::EndPopup();
-  }
   DisplayField2D(m_model, ImGui::GetWindowContentRegionMax() -
                               ImGui::GetWindowContentRegionMin());
 }
+
+void Field2DView::Settings() {
+  DisplayField2DSettings(m_model);
+}
+
+bool Field2DView::HasSettings() {
+  return true;
+}

glass/src/lib/native/cpp/other/Log.cpp

@@ -62,18 +62,21 @@ void glass::DisplayLog(LogData* data, bool autoScroll) {
 }
 
 void LogView::Display() {
-  if (ImGui::BeginPopupContextItem()) {
-    ImGui::Checkbox("Auto-scroll", &m_autoScroll);
-    if (ImGui::Selectable("Clear")) {
-      m_data->Clear();
-    }
-    const auto& buf = m_data->GetBuffer();
-    if (ImGui::Selectable("Copy to Clipboard", false,
-                          buf.empty() ? ImGuiSelectableFlags_Disabled : 0)) {
-      ImGui::SetClipboardText(buf.c_str());
-    }
-    ImGui::EndPopup();
-  }
-
   DisplayLog(m_data, m_autoScroll);
 }
+
+void LogView::Settings() {
+  ImGui::Checkbox("Auto-scroll", &m_autoScroll);
+  if (ImGui::Selectable("Clear")) {
+    m_data->Clear();
+  }
+  const auto& buf = m_data->GetBuffer();
+  if (ImGui::Selectable("Copy to Clipboard", false,
+                        buf.empty() ? ImGuiSelectableFlags_Disabled : 0)) {
+    ImGui::SetClipboardText(buf.c_str());
+  }
+}
+
+bool LogView::HasSettings() {
+  return true;
+}

glass/src/lib/native/cpp/other/Mechanism2D.cpp

@@ -249,10 +249,14 @@ void glass::DisplayMechanism2D(Mechanism2DModel* model,
 }
 
 void Mechanism2DView::Display() {
-  if (ImGui::BeginPopupContextItem()) {
-    DisplayMechanism2DSettings(m_model);
-    ImGui::EndPopup();
-  }
   DisplayMechanism2D(m_model, ImGui::GetWindowContentRegionMax() -
                                   ImGui::GetWindowContentRegionMin());
 }
+
+void Mechanism2DView::Settings() {
+  DisplayMechanism2DSettings(m_model);
+}
+
+bool Mechanism2DView::HasSettings() {
+  return true;
+}

glass/src/lib/native/cpp/other/Plot.cpp

@@ -182,6 +182,8 @@ class PlotView : public View {
   PlotView(PlotProvider* provider, Storage& storage);
 
   void Display() override;
+  void Settings() override;
+  bool HasSettings() override;
 
   void MovePlot(PlotView* fromView, size_t fromIndex, size_t toIndex);
 
@@ -767,71 +769,6 @@ PlotView::PlotView(PlotProvider* provider, Storage& storage)
 }
 
 void PlotView::Display() {
-  if (ImGui::BeginPopupContextItem()) {
-    if (ImGui::Button("Add plot")) {
-      m_plotsStorage.emplace_back(std::make_unique<Storage>());
-      m_plots.emplace_back(std::make_unique<Plot>(*m_plotsStorage.back()));
-    }
-
-    for (size_t i = 0; i < m_plots.size(); ++i) {
-      auto& plot = m_plots[i];
-      ImGui::PushID(i);
-
-      char name[64];
-      if (!plot->GetName().empty()) {
-        std::snprintf(name, sizeof(name), "%s", plot->GetName().c_str());
-      } else {
-        std::snprintf(name, sizeof(name), "Plot %d", static_cast<int>(i));
-      }
-
-      char label[90];
-      std::snprintf(label, sizeof(label), "%s###header%d", name,
-                    static_cast<int>(i));
-
-      bool open = ImGui::CollapsingHeader(label);
-
-      // DND source and target for Plot
-      if (ImGui::BeginDragDropSource()) {
-        PlotSeriesRef ref = {this, i, 0};
-        ImGui::SetDragDropPayload("Plot", &ref, sizeof(ref));
-        ImGui::TextUnformatted(name);
-        ImGui::EndDragDropSource();
-      }
-      plot->DragDropTarget(*this, i, false);
-
-      if (open) {
-        if (ImGui::Button("Move Up")) {
-          if (i > 0) {
-            std::swap(m_plotsStorage[i - 1], m_plotsStorage[i]);
-            std::swap(m_plots[i - 1], plot);
-          }
-        }
-
-        ImGui::SameLine();
-        if (ImGui::Button("Move Down")) {
-          if (i < (m_plots.size() - 1)) {
-            std::swap(m_plotsStorage[i], m_plotsStorage[i + 1]);
-            std::swap(plot, m_plots[i + 1]);
-          }
-        }
-
-        ImGui::SameLine();
-        if (ImGui::Button("Delete")) {
-          m_plotsStorage.erase(m_plotsStorage.begin() + i);
-          m_plots.erase(m_plots.begin() + i);
-          ImGui::PopID();
-          continue;
-        }
-
-        plot->EmitSettings(i);
-      }
-
-      ImGui::PopID();
-    }
-
-    ImGui::EndPopup();
-  }
-
   if (m_plots.empty()) {
     if (ImGui::Button("Add plot")) {
       m_plotsStorage.emplace_back(std::make_unique<Storage>());
@@ -968,6 +905,73 @@ PlotProvider::PlotProvider(Storage& storage) : WindowManager{storage} {
   });
 }
 
+void PlotView::Settings() {
+  if (ImGui::Button("Add plot")) {
+    m_plotsStorage.emplace_back(std::make_unique<Storage>());
+    m_plots.emplace_back(std::make_unique<Plot>(*m_plotsStorage.back()));
+  }
+
+  for (size_t i = 0; i < m_plots.size(); ++i) {
+    auto& plot = m_plots[i];
+    ImGui::PushID(i);
+
+    char name[64];
+    if (!plot->GetName().empty()) {
+      std::snprintf(name, sizeof(name), "%s", plot->GetName().c_str());
+    } else {
+      std::snprintf(name, sizeof(name), "Plot %d", static_cast<int>(i));
+    }
+
+    char label[90];
+    std::snprintf(label, sizeof(label), "%s###header%d", name,
+                  static_cast<int>(i));
+
+    bool open = ImGui::CollapsingHeader(label);
+
+    // DND source and target for Plot
+    if (ImGui::BeginDragDropSource()) {
+      PlotSeriesRef ref = {this, i, 0};
+      ImGui::SetDragDropPayload("Plot", &ref, sizeof(ref));
+      ImGui::TextUnformatted(name);
+      ImGui::EndDragDropSource();
+    }
+    plot->DragDropTarget(*this, i, false);
+
+    if (open) {
+      if (ImGui::Button("Move Up")) {
+        if (i > 0) {
+          std::swap(m_plotsStorage[i - 1], m_plotsStorage[i]);
+          std::swap(m_plots[i - 1], plot);
+        }
+      }
+
+      ImGui::SameLine();
+      if (ImGui::Button("Move Down")) {
+        if (i < (m_plots.size() - 1)) {
+          std::swap(m_plotsStorage[i], m_plotsStorage[i + 1]);
+          std::swap(plot, m_plots[i + 1]);
+        }
+      }
+
+      ImGui::SameLine();
+      if (ImGui::Button("Delete")) {
+        m_plotsStorage.erase(m_plotsStorage.begin() + i);
+        m_plots.erase(m_plots.begin() + i);
+        ImGui::PopID();
+        continue;
+      }
+
+      plot->EmitSettings(i);
+    }
+
+    ImGui::PopID();
+  }
+}
+
+bool PlotView::HasSettings() {
+  return true;
+}
+
 PlotProvider::~PlotProvider() = default;
 
 void PlotProvider::DisplayMenu() {

glass/src/lib/native/cpp/support/ExtraGuiWidgets.cpp

@@ -4,6 +4,8 @@
 
 #include "glass/support/ExtraGuiWidgets.h"
 
+#include <imgui.h>
+
 #define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui_internal.h>
 
@@ -174,4 +176,46 @@ bool HeaderDeleteButton(const char* label) {
   return rv;
 }
 
+bool HamburgerButton(const ImGuiID id, const ImVec2 position) {
+  const ImGuiStyle& style = ImGui::GetStyle();
+
+  ImGuiWindow* window = ImGui::GetCurrentWindow();
+
+  // Frame padding on both sides, then one character in the middle
+  const ImRect bb{
+      position, position + ImVec2(ImGui::GetFontSize(), ImGui::GetFontSize()) +
+                    style.FramePadding * 2.0f};
+
+  ImGui::ItemAdd(bb, id);
+
+  bool hovered, held;
+  bool pressed = ImGui::ButtonBehavior(bb, id, &hovered, &held);
+
+  const ImU32 bgCol =
+      ImGui::GetColorU32(held ? ImGuiCol_ButtonActive : ImGuiCol_ButtonHovered);
+  const ImVec2 center = bb.GetCenter();
+  if (hovered) {
+    window->DrawList->AddCircleFilled(
+        center, ImMax(2.0f, ImGui::GetFontSize() * 0.5f + 1.0f), bgCol, 12);
+  }
+
+  const ImU32 fgCol = ImGui::GetColorU32(ImGuiCol_Text);
+
+  const float halfLineWidth = ImGui::GetFontSize() * 0.5 * 0.7071;
+  const float halfTotalHeight = halfLineWidth * 0.875;
+  ImVec2 lineStart = {center.x - halfLineWidth, center.y - halfTotalHeight};
+  ImVec2 lineEnd = {center.x + halfLineWidth, center.y - halfTotalHeight};
+
+  ImVec2 increment = {0.0, halfTotalHeight};
+
+  for (int i = 0; i < 3; i++) {
+    window->DrawList->AddLine(lineStart, lineEnd, fgCol);
+
+    lineStart += increment;
+    lineEnd += increment;
+  }
+
+  return pressed;
+}
+
 }  // namespace glass

glass/src/lib/native/include/glass/Context.h

@@ -201,4 +201,6 @@ void PopID();
 
 bool PopupEditName(const char* label, std::string* name);
 
+bool ItemEditName(std::string* name);
+
 }  // namespace glass

glass/src/lib/native/include/glass/View.h

@@ -35,6 +35,17 @@ class View {
    * ImGui::Begin() returns false).
    */
   virtual void Hidden();
+
+  /**
+   * Called from within ImGui::BeginContextPopupItem() and ImGui::EndPopup().
+   * Used to display the settings for the view
+   */
+  virtual void Settings();
+
+  /**
+   * If the view has settings and if the result of Settings should be displayed.
+   */
+  virtual bool HasSettings();
 };
 
 /**

glass/src/lib/native/include/glass/other/FMS.h

@@ -38,7 +38,7 @@ class FMSModel : public Model {
   virtual void SetEnabled(bool val) = 0;
   virtual void SetTest(bool val) = 0;
   virtual void SetAutonomous(bool val) = 0;
-  virtual void SetGameSpecificMessage(const char* val) = 0;
+  virtual void SetGameSpecificMessage(std::string_view val) = 0;
 };
 
 /**

glass/src/lib/native/include/glass/other/Field2D.h

@@ -46,6 +46,8 @@ class Field2DView : public View {
   explicit Field2DView(Field2DModel* model) : m_model{model} {}
 
   void Display() override;
+  void Settings() override;
+  bool HasSettings() override;
 
  private:
   Field2DModel* m_model;

glass/src/lib/native/include/glass/other/Log.h

@@ -35,6 +35,8 @@ class LogView : public View {
   explicit LogView(LogData* data) : m_data{data} {}
 
   void Display() override;
+  void Settings() override;
+  bool HasSettings() override;
 
  private:
   LogData* m_data;

glass/src/lib/native/include/glass/other/Mechanism2D.h

@@ -55,6 +55,8 @@ class Mechanism2DView : public View {
   explicit Mechanism2DView(Mechanism2DModel* model) : m_model{model} {}
 
   void Display() override;
+  void Settings() override;
+  bool HasSettings() override;
 
  private:
   Mechanism2DModel* m_model;

glass/src/lib/native/include/glass/support/ExtraGuiWidgets.h

@@ -93,4 +93,9 @@ bool DeleteButton(ImGuiID id, const ImVec2& pos);
  */
 bool HeaderDeleteButton(const char* label);
 
+/**
+ * Settings button similar to ImGui::CloseButton.
+ */
+bool HamburgerButton(const ImGuiID id, const ImVec2 position);
+
 }  // namespace glass

glass/src/libnt/native/cpp/NTDigitalInput.cpp

@@ -15,8 +15,8 @@ NTDigitalInputModel::NTDigitalInputModel(std::string_view path)
 NTDigitalInputModel::NTDigitalInputModel(nt::NetworkTableInstance inst,
                                          std::string_view path)
     : m_inst{inst},
-      m_value{inst.GetBooleanTopic(fmt::format("{}/Value", path))
-                  .Subscribe(false, {{nt::PubSubOption::SendAll(true)}})},
+      m_value{
+          inst.GetBooleanTopic(fmt::format("{}/Value", path)).Subscribe(false)},
       m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
       m_valueData{fmt::format("NT_DIn:{}", path)},
       m_nameValue{wpi::rsplit(path, '/').second} {

glass/src/libnt/native/cpp/NTDigitalOutput.cpp

@@ -14,8 +14,8 @@ NTDigitalOutputModel::NTDigitalOutputModel(std::string_view path)
 NTDigitalOutputModel::NTDigitalOutputModel(nt::NetworkTableInstance inst,
                                            std::string_view path)
     : m_inst{inst},
-      m_value{inst.GetBooleanTopic(fmt::format("{}/Value", path))
-                  .GetEntry(false, {{nt::PubSubOption::SendAll(true)}})},
+      m_value{
+          inst.GetBooleanTopic(fmt::format("{}/Value", path)).GetEntry(false)},
       m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
       m_controllable{inst.GetBooleanTopic(fmt::format("{}/.controllable", path))
                          .Subscribe(false)},

glass/src/libnt/native/cpp/NTFMS.cpp

@@ -21,11 +21,11 @@ NTFMSModel::NTFMSModel(nt::NetworkTableInstance inst, std::string_view path)
           inst.GetStringTopic(fmt::format("{}/GameSpecificMessage", path))
               .Subscribe("")},
       m_alliance{inst.GetBooleanTopic(fmt::format("{}/IsRedAlliance", path))
-                     .Subscribe(false, {{nt::PubSubOption::SendAll(true)}})},
+                     .Subscribe(false)},
       m_station{inst.GetIntegerTopic(fmt::format("{}/StationNumber", path))
-                    .Subscribe(0, {{nt::PubSubOption::SendAll(true)}})},
+                    .Subscribe(0)},
       m_controlWord{inst.GetIntegerTopic(fmt::format("{}/FMSControlData", path))
-                        .Subscribe(0, {{nt::PubSubOption::SendAll(true)}})},
+                        .Subscribe(0)},
       m_fmsAttached{fmt::format("NT_FMS:FMSAttached:{}", path)},
       m_dsAttached{fmt::format("NT_FMS:DSAttached:{}", path)},
       m_allianceStationId{fmt::format("NT_FMS:AllianceStationID:{}", path)},

glass/src/libnt/native/cpp/NTField2D.cpp

@@ -112,10 +112,7 @@ NTField2DModel::NTField2DModel(nt::NetworkTableInstance inst,
                                std::string_view path)
     : m_path{fmt::format("{}/", path)},
       m_inst{inst},
-      m_tableSub{inst,
-                 {{m_path}},
-                 {{nt::PubSubOption::SendAll(true),
-                   nt::PubSubOption::Periodic(0.05)}}},
+      m_tableSub{inst, {{m_path}}, {.periodic = 0.05, .sendAll = true}},
       m_nameTopic{inst.GetTopic(fmt::format("{}/.name", path))},
       m_poller{inst} {
   m_poller.AddListener(m_tableSub, nt::EventFlags::kTopic |

glass/src/libnt/native/cpp/NTGyro.cpp

@@ -14,8 +14,7 @@ NTGyroModel::NTGyroModel(std::string_view path)
 
 NTGyroModel::NTGyroModel(nt::NetworkTableInstance inst, std::string_view path)
     : m_inst{inst},
-      m_angle{inst.GetDoubleTopic(fmt::format("{}/Value", path))
-                  .Subscribe(0, {{nt::PubSubOption::SendAll(true)}})},
+      m_angle{inst.GetDoubleTopic(fmt::format("{}/Value", path)).Subscribe(0)},
       m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe({})},
       m_angleData{fmt::format("NT_Gyro:{}", path)},
       m_nameValue{wpi::rsplit(path, '/').second} {}

glass/src/libnt/native/cpp/NTMecanumDrive.cpp

@@ -22,16 +22,16 @@ NTMecanumDriveModel::NTMecanumDriveModel(nt::NetworkTableInstance inst,
                          .Subscribe(0)},
       m_flPercent{
           inst.GetDoubleTopic(fmt::format("{}/Front Left Motor Speed", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+              .GetEntry(0)},
       m_frPercent{
           inst.GetDoubleTopic(fmt::format("{}/Front Right Motor Speed", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+              .GetEntry(0)},
       m_rlPercent{
           inst.GetDoubleTopic(fmt::format("{}/Rear Left Motor Speed", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+              .GetEntry(0)},
       m_rrPercent{
           inst.GetDoubleTopic(fmt::format("{}/Rear Right Motor Speed", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+              .GetEntry(0)},
       m_nameValue{wpi::rsplit(path, '/').second},
       m_flPercentData{fmt::format("NTMcnmDriveFL:{}", path)},
       m_frPercentData{fmt::format("NTMcnmDriveFR:{}", path)},

glass/src/libnt/native/cpp/NTMechanism2D.cpp

@@ -240,10 +240,7 @@ NTMechanism2DModel::NTMechanism2DModel(nt::NetworkTableInstance inst,
                                        std::string_view path)
     : m_inst{inst},
       m_path{fmt::format("{}/", path)},
-      m_tableSub{inst,
-                 {{m_path}},
-                 {{nt::PubSubOption::SendAll(true),
-                   nt::PubSubOption::Periodic(0.05)}}},
+      m_tableSub{inst, {{m_path}}, {.periodic = 0.05, .sendAll = true}},
       m_nameTopic{m_inst.GetTopic(fmt::format("{}/.name", path))},
       m_dimensionsTopic{m_inst.GetTopic(fmt::format("{}/dims", path))},
       m_bgColorTopic{m_inst.GetTopic(fmt::format("{}/backgroundColor", path))},

glass/src/libnt/native/cpp/NTMotorController.cpp

@@ -15,8 +15,7 @@ NTMotorControllerModel::NTMotorControllerModel(std::string_view path)
 NTMotorControllerModel::NTMotorControllerModel(nt::NetworkTableInstance inst,
                                                std::string_view path)
     : m_inst{inst},
-      m_value{inst.GetDoubleTopic(fmt::format("{}/Value", path))
-                  .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+      m_value{inst.GetDoubleTopic(fmt::format("{}/Value", path)).GetEntry(0)},
       m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
       m_controllable{inst.GetBooleanTopic(fmt::format("{}/.controllable", path))
                          .Subscribe(false)},

glass/src/libnt/native/cpp/NTPIDController.cpp

@@ -18,14 +18,11 @@ NTPIDControllerModel::NTPIDControllerModel(nt::NetworkTableInstance inst,
       m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
       m_controllable{inst.GetBooleanTopic(fmt::format("{}/.controllable", path))
                          .Subscribe(false)},
-      m_p{inst.GetDoubleTopic(fmt::format("{}/p", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
-      m_i{inst.GetDoubleTopic(fmt::format("{}/i", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
-      m_d{inst.GetDoubleTopic(fmt::format("{}/d", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
-      m_setpoint{inst.GetDoubleTopic(fmt::format("{}/setpoint", path))
-                     .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+      m_p{inst.GetDoubleTopic(fmt::format("{}/p", path)).GetEntry(0)},
+      m_i{inst.GetDoubleTopic(fmt::format("{}/i", path)).GetEntry(0)},
+      m_d{inst.GetDoubleTopic(fmt::format("{}/d", path)).GetEntry(0)},
+      m_setpoint{
+          inst.GetDoubleTopic(fmt::format("{}/setpoint", path)).GetEntry(0)},
       m_pData{fmt::format("NTPIDCtrlP:{}", path)},
       m_iData{fmt::format("NTPIDCtrlI:{}", path)},
       m_dData{fmt::format("NTPIDCtrlD:{}", path)},

glass/src/libnt/native/cpp/NetworkTables.cpp

@@ -478,6 +478,10 @@ void NetworkTablesModel::Update() {
             entry->info.type_str == "msgpack") {
           // meta topic handling
           if (entry->info.name == "$clients") {
+            // need to remove deleted entries as UpdateClients() uses GetEntry()
+            if (updateTree) {
+              std::erase(m_sortedEntries, nullptr);
+            }
             UpdateClients(entry->value.GetRaw());
           } else if (entry->info.name == "$serverpub") {
             m_server.UpdatePublishers(entry->value.GetRaw());
@@ -505,9 +509,7 @@ void NetworkTablesModel::Update() {
   }
 
   // remove deleted entries
-  m_sortedEntries.erase(
-      std::remove(m_sortedEntries.begin(), m_sortedEntries.end(), nullptr),
-      m_sortedEntries.end());
+  std::erase(m_sortedEntries, nullptr);
 
   RebuildTree();
 }
@@ -598,138 +600,42 @@ NetworkTablesModel::Entry* NetworkTablesModel::AddEntry(NT_Topic topic) {
   return entry.get();
 }
 
+NetworkTablesModel::Client::Subscriber::Subscriber(
+    nt::meta::ClientSubscriber&& oth)
+    : ClientSubscriber{std::move(oth)},
+      topicsStr{StringArrayToString(topics)} {}
+
 void NetworkTablesModel::Client::UpdatePublishers(
     std::span<const uint8_t> data) {
-  mpack_reader_t r;
-  mpack_reader_init_data(&r, data);
-  uint32_t numPub = mpack_expect_array_max(&r, 1000);
-  std::vector<ClientPublisher> newPublishers;
-  newPublishers.reserve(numPub);
-  for (uint32_t i = 0; i < numPub; ++i) {
-    ClientPublisher pub;
-    uint32_t numMapElem = mpack_expect_map(&r);
-    for (uint32_t j = 0; j < numMapElem; ++j) {
-      std::string key;
-      mpack_expect_str(&r, &key);
-      if (key == "uid") {
-        pub.uid = mpack_expect_i64(&r);
-      } else if (key == "topic") {
-        mpack_expect_str(&r, &pub.topic);
-      } else {
-        mpack_discard(&r);
-      }
-    }
-    mpack_done_map(&r);
-    newPublishers.emplace_back(std::move(pub));
-  }
-  mpack_done_array(&r);
-  if (mpack_reader_destroy(&r) == mpack_ok) {
-    publishers = std::move(newPublishers);
+  if (auto pubs = nt::meta::DecodeClientPublishers(data)) {
+    publishers = std::move(*pubs);
   } else {
     fmt::print(stderr, "Failed to update publishers\n");
   }
 }
 
-static void DecodeSubscriberOptions(
-    mpack_reader_t& r, NetworkTablesModel::SubscriberOptions* options) {
-  *options = NetworkTablesModel::SubscriberOptions{};
-  uint32_t numMapElem = mpack_expect_map(&r);
-  for (uint32_t j = 0; j < numMapElem; ++j) {
-    std::string key;
-    mpack_expect_str(&r, &key);
-    if (key == "topicsonly") {
-      options->topicsOnly = mpack_expect_bool(&r);
-    } else if (key == "all") {
-      options->sendAll = mpack_expect_bool(&r);
-    } else if (key == "periodic") {
-      options->periodic = mpack_expect_float(&r);
-    } else if (key == "prefix") {
-      options->prefixMatch = mpack_expect_bool(&r);
-    } else {
-      // TODO: Save other options
-      mpack_discard(&r);
-    }
-  }
-  mpack_done_map(&r);
-}
-
 void NetworkTablesModel::Client::UpdateSubscribers(
     std::span<const uint8_t> data) {
-  mpack_reader_t r;
-  mpack_reader_init_data(&r, data);
-  uint32_t numSub = mpack_expect_array_max(&r, 1000);
-  std::vector<ClientSubscriber> newSubscribers;
-  newSubscribers.reserve(numSub);
-  for (uint32_t i = 0; i < numSub; ++i) {
-    ClientSubscriber sub;
-    uint32_t numMapElem = mpack_expect_map(&r);
-    for (uint32_t j = 0; j < numMapElem; ++j) {
-      std::string key;
-      mpack_expect_str(&r, &key);
-      if (key == "uid") {
-        sub.uid = mpack_expect_i64(&r);
-      } else if (key == "topics") {
-        uint32_t numPrefix = mpack_expect_array_max(&r, 100);
-        sub.topics.reserve(numPrefix);
-        for (uint32_t k = 0; k < numPrefix; ++k) {
-          std::string val;
-          if (mpack_expect_str(&r, &val) == mpack_ok) {
-            sub.topics.emplace_back(std::move(val));
-          }
-        }
-        sub.topicsStr = StringArrayToString(sub.topics);
-        mpack_done_array(&r);
-      } else if (key == "options") {
-        DecodeSubscriberOptions(r, &sub.options);
-      } else {
-        mpack_discard(&r);
-      }
+  if (auto subs = nt::meta::DecodeClientSubscribers(data)) {
+    subscribers.clear();
+    subscribers.reserve(subs->size());
+    for (auto&& sub : *subs) {
+      subscribers.emplace_back(std::move(sub));
     }
-    mpack_done_map(&r);
-    newSubscribers.emplace_back(std::move(sub));
-  }
-  mpack_done_array(&r);
-  if (mpack_reader_destroy(&r) == mpack_ok) {
-    subscribers = std::move(newSubscribers);
   } else {
     fmt::print(stderr, "Failed to update subscribers\n");
   }
 }
 
 void NetworkTablesModel::UpdateClients(std::span<const uint8_t> data) {
-  mpack_reader_t r;
-  mpack_reader_init_data(&r, data);
-  uint32_t numClients = mpack_expect_array_max(&r, 100);
-  std::vector<Client> clientsArr;
-  clientsArr.reserve(numClients);
-  for (uint32_t i = 0; i < numClients; ++i) {
-    Client client;
-    uint32_t numMapElem = mpack_expect_map(&r);
-    for (uint32_t j = 0; j < numMapElem; ++j) {
-      std::string key;
-      mpack_expect_str(&r, &key);
-      if (key == "id") {
-        mpack_expect_str(&r, &client.id);
-      } else if (key == "conn") {
-        mpack_expect_str(&r, &client.conn);
-      } else if (key == "ver") {
-        uint16_t val = mpack_expect_u16(&r);
-        client.version = fmt::format("{}.{}", val >> 8, val & 0xff);
-      } else {
-        mpack_discard(&r);
-      }
-    }
-    mpack_done_map(&r);
-    clientsArr.emplace_back(std::move(client));
-  }
-  mpack_done_array(&r);
-  if (mpack_reader_destroy(&r) != mpack_ok) {
+  auto clientsArr = nt::meta::DecodeClients(data);
+  if (!clientsArr) {
     return;
   }
 
   // we need to create a new map so deletions are reflected
   std::map<std::string, Client, std::less<>> newClients;
-  for (auto&& client : clientsArr) {
+  for (auto&& client : *clientsArr) {
     auto& newClient = newClients[client.id];
     newClient = std::move(client);
     auto it = m_clients.find(newClient.id);
@@ -1497,8 +1403,8 @@ void glass::DisplayNetworkTablesInfo(NetworkTablesModel* model) {
         if (CollapsingHeader(client.second.id.c_str())) {
           PushID(client.second.id.c_str());
           ImGui::Indent();
-          ImGui::Text("%s (version %s)", client.second.conn.c_str(),
-                      client.second.version.c_str());
+          ImGui::Text("%s (version %u.%u)", client.second.conn.c_str(),
+                      client.second.version >> 8, client.second.version & 0xff);
           DisplayClient(client.second);
           ImGui::Unindent();
           PopID();
@@ -1600,9 +1506,13 @@ void NetworkTablesFlagsSettings::DisplayMenu() {
 
 void NetworkTablesView::Display() {
   m_flags.Update();
-  if (ImGui::BeginPopupContextItem()) {
-    m_flags.DisplayMenu();
-    ImGui::EndPopup();
-  }
   DisplayNetworkTables(m_model, m_flags.GetFlags());
 }
+
+void NetworkTablesView::Settings() {
+  m_flags.DisplayMenu();
+}
+
+bool NetworkTablesView::HasSettings() {
+  return true;
+}

glass/src/libnt/native/cpp/NetworkTablesSettings.cpp

@@ -66,25 +66,27 @@ void NetworkTablesSettings::Thread::Main() {
       } else if (m_mode == 2) {
         nt::StartClient3(m_inst, m_clientName);
       }
+
+      unsigned int port = m_mode == 1 ? m_port4 : m_port3;
       if (!wpi::contains(serverTeam, '.') &&
           (team = wpi::parse_integer<unsigned int>(serverTeam, 10))) {
-        nt::SetServerTeam(m_inst, team.value(), 0);
+        nt::SetServerTeam(m_inst, team.value(), port);
       } else {
         wpi::SmallVector<std::string_view, 4> serverNames;
         std::vector<std::pair<std::string_view, unsigned int>> servers;
         wpi::split(serverTeam, serverNames, ',', -1, false);
         for (auto&& serverName : serverNames) {
-          servers.emplace_back(serverName, 0);
+          servers.emplace_back(serverName, port);
         }
         nt::SetServer(m_inst, servers);
       }
 
       if (m_dsClient) {
-        nt::StartDSClient(m_inst, 0);
+        nt::StartDSClient(m_inst, port);
       }
     } else if (m_mode == 3) {
       nt::StartServer(m_inst, m_iniName.c_str(), m_listenAddress.c_str(),
-                      NT_DEFAULT_PORT3, NT_DEFAULT_PORT4);
+                      m_port3, m_port4);
     }
   }
 }
@@ -99,6 +101,8 @@ NetworkTablesSettings::NetworkTablesSettings(std::string_view clientName,
       m_serverTeam{storage.GetString("serverTeam")},
       m_listenAddress{storage.GetString("listenAddress")},
       m_clientName{storage.GetString("clientName", clientName)},
+      m_port3{storage.GetInt("port3", NT_DEFAULT_PORT3)},
+      m_port4{storage.GetInt("port4", NT_DEFAULT_PORT4)},
       m_dsClient{storage.GetBool("dsClient", true)} {
   m_thread.Start(inst);
 }
@@ -117,21 +121,54 @@ void NetworkTablesSettings::Update() {
   thr->m_serverTeam = m_serverTeam;
   thr->m_listenAddress = m_listenAddress;
   thr->m_clientName = m_clientName;
+  thr->m_port3 = m_port3;
+  thr->m_port4 = m_port4;
   thr->m_dsClient = m_dsClient;
   thr->m_cond.notify_one();
 }
 
+static void LimitPortRange(int* port) {
+  if (*port < 0) {
+    *port = 0;
+  } else if (*port > 65535) {
+    *port = 65535;
+  }
+}
+
 bool NetworkTablesSettings::Display() {
   m_mode.Combo("Mode", m_serverOption ? 4 : 3);
   switch (m_mode.GetValue()) {
     case 1:
-    case 2:
+    case 2: {
       ImGui::InputText("Team/IP", &m_serverTeam);
+      int* port = m_mode.GetValue() == 1 ? &m_port4 : &m_port3;
+      if (ImGui::InputInt("Port", port)) {
+        LimitPortRange(port);
+      }
+      ImGui::SameLine();
+      if (ImGui::SmallButton("Default")) {
+        *port = m_mode.GetValue() == 1 ? NT_DEFAULT_PORT4 : NT_DEFAULT_PORT3;
+      }
       ImGui::InputText("Network Identity", &m_clientName);
       ImGui::Checkbox("Get Address from DS", &m_dsClient);
       break;
+    }
     case 3:
       ImGui::InputText("Listen Address", &m_listenAddress);
+      if (ImGui::InputInt("NT3 port", &m_port3)) {
+        LimitPortRange(&m_port3);
+      }
+      ImGui::SameLine();
+      if (ImGui::SmallButton("Default##default3")) {
+        m_port3 = NT_DEFAULT_PORT3;
+      }
+      if (ImGui::InputInt("NT4 port", &m_port4)) {
+        LimitPortRange(&m_port4);
+      }
+      ImGui::SameLine();
+      if (ImGui::SmallButton("Default##default4")) {
+        m_port4 = NT_DEFAULT_PORT4;
+      }
       ImGui::InputText("Persistent Filename", &m_persistentFilename);
       break;
     default:

glass/src/libnt/native/include/glass/networktables/NTFMS.h

@@ -47,7 +47,7 @@ class NTFMSModel : public FMSModel {
   void SetEnabled(bool val) override {}
   void SetTest(bool val) override {}
   void SetAutonomous(bool val) override {}
-  void SetGameSpecificMessage(const char* val) override {}
+  void SetGameSpecificMessage(std::string_view val) override {}
 
   void Update() override;
   bool Exists() override;

glass/src/libnt/native/include/glass/networktables/NetworkTables.h

@@ -28,25 +28,6 @@ class DataSource;
 
 class NetworkTablesModel : public Model {
  public:
-  struct SubscriberOptions {
-    float periodic = 0.1f;
-    bool topicsOnly = false;
-    bool sendAll = false;
-    bool prefixMatch = false;
-    // std::string otherStr;
-  };
-
-  struct TopicPublisher {
-    std::string client;
-    uint64_t pubuid;
-  };
-
-  struct TopicSubscriber {
-    std::string client;
-    uint64_t subuid;
-    SubscriberOptions options;
-  };
-
   struct EntryValueTreeNode;
 
   struct ValueSource {
@@ -103,8 +84,8 @@ class NetworkTablesModel : public Model {
     /** Publisher (created when the value changes). */
     NT_Publisher publisher{0};
 
-    std::vector<TopicPublisher> publishers;
-    std::vector<TopicSubscriber> subscribers;
+    std::vector<nt::meta::TopicPublisher> publishers;
+    std::vector<nt::meta::TopicSubscriber> subscribers;
   };
 
   struct TreeNode {
@@ -126,24 +107,18 @@ class NetworkTablesModel : public Model {
     std::vector<TreeNode> children;
   };
 
-  struct ClientPublisher {
-    int64_t uid = -1;
-    std::string topic;
-  };
+  struct Client : public nt::meta::Client {
+    Client() = default;
+    /*implicit*/ Client(nt::meta::Client&& oth)  // NOLINT
+        : nt::meta::Client{std::move(oth)} {}
 
-  struct ClientSubscriber {
-    int64_t uid = -1;
-    std::vector<std::string> topics;
-    std::string topicsStr;
-    SubscriberOptions options;
-  };
+    struct Subscriber : public nt::meta::ClientSubscriber {
+      /*implicit*/ Subscriber(nt::meta::ClientSubscriber&& oth);  // NOLINT
+      std::string topicsStr;
+    };
 
-  struct Client {
-    std::string id;
-    std::string conn;
-    std::string version;
-    std::vector<ClientPublisher> publishers;
-    std::vector<ClientSubscriber> subscribers;
+    std::vector<nt::meta::ClientPublisher> publishers;
+    std::vector<Subscriber> subscribers;
 
     void UpdatePublishers(std::span<const uint8_t> data);
     void UpdateSubscribers(std::span<const uint8_t> data);
@@ -251,6 +226,8 @@ class NetworkTablesView : public View {
       : m_model{model}, m_flags{defaultFlags} {}
 
   void Display() override;
+  void Settings() override;
+  bool HasSettings() override;
 
  private:
   NetworkTablesModel* m_model;

glass/src/libnt/native/include/glass/networktables/NetworkTablesSettings.h

@@ -41,6 +41,8 @@ class NetworkTablesSettings {
   std::string& m_serverTeam;
   std::string& m_listenAddress;
   std::string& m_clientName;
+  int& m_port3;
+  int& m_port4;
   bool& m_dsClient;
 
   class Thread : public wpi::SafeThread {
@@ -56,6 +58,8 @@ class NetworkTablesSettings {
     std::string m_serverTeam;
     std::string m_listenAddress;
     std::string m_clientName;
+    int m_port3;
+    int m_port4;
     bool m_dsClient;
   };
   wpi::SafeThreadOwner<Thread> m_thread;

gradle.properties

@@ -1,8 +1 @@
-# The --add-exports flags work around a bug with spotless and JDK 17
-# https://github.com/diffplug/spotless/issues/834
-org.gradle.jvmargs=-Xmx2g \
-  --add-exports jdk.compiler/com.sun.tools.javac.api=ALL-UNNAMED \
-  --add-exports jdk.compiler/com.sun.tools.javac.file=ALL-UNNAMED \
-  --add-exports jdk.compiler/com.sun.tools.javac.parser=ALL-UNNAMED \
-  --add-exports jdk.compiler/com.sun.tools.javac.tree=ALL-UNNAMED \
-  --add-exports jdk.compiler/com.sun.tools.javac.util=ALL-UNNAMED
+org.gradle.jvmargs=-Xmx2g

hal/src/dev/native/cpp/main.cpp

@@ -8,5 +8,5 @@
 
 int main() {
   fmt::print("Hello World\n");
-  fmt::print("{}\n", HAL_GetRuntimeType());
+  fmt::print("{}\n", static_cast<int32_t>(HAL_GetRuntimeType()));
 }

hal/src/main/java/edu/wpi/first/hal/DIOJNI.java

@@ -39,5 +39,7 @@ public class DIOJNI extends JNIWrapper {
 
   public static native void setDigitalPWMDutyCycle(int pwmGenerator, double dutyCycle);
 
+  public static native void setDigitalPWMPPS(int pwmGenerator, double dutyCycle);
+
   public static native void setDigitalPWMOutputChannel(int pwmGenerator, int channel);
 }

hal/src/main/java/edu/wpi/first/hal/HALUtil.java

@@ -24,6 +24,8 @@ public final class HALUtil extends JNIWrapper {
 
   public static native String getSerialNumber();
 
+  public static native String getComments();
+
   public static native long getFPGATime();
 
   public static native int getHALRuntimeType();