[wpimath] Fix DCMotor.getSpeed() (#5061)

This bug didn't occur in C++ because the units system caught it at
compile time.

.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

@@ -9,6 +9,8 @@ simgui-ds.json
 simgui-window.json
 simgui.json
 
+networktables.json
+
 # Created by the jenkins test script
 test-reports
 
@@ -19,6 +21,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 {

CONTRIBUTING.md

@@ -40,8 +40,39 @@ So you want to contribute your changes back to WPILib. Great! We have a few cont
 WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system.
 While the library should be fully formatted according to the styles, additional elements of the style guide were not followed when the library was initially created. All new code should follow the guidelines. If you are looking for some easy ramp-up tasks, finding areas that don't follow the style guide and fixing them is very welcome.
 
+### Math documentation
+
 When writing math expressions in documentation, use https://www.unicodeit.net/ to convert LaTeX to a Unicode equivalent that's easier to read. Not all expressions will translate (e.g., superscripts of superscripts) so focus on making it readable by someone who isn't familiar with LaTeX. If content on multiple lines needs to be aligned in Doxygen/Javadoc comments (e.g., integration/summation limits, matrices packed with square brackets and superscripts for them), put them in @verbatim/@endverbatim blocks in Doxygen or `<pre>` tags in Javadoc so they render with monospace font.
 
+The LaTeX to Unicode conversions can also be done locally via the unicodeit Python package. To install it, execute:
+```bash
+pip install --user unicodeit
+```
+
+Here's example usage:
+```bash
+$ python -m unicodeit.cli 'x_{k+1} = Ax_k + Bu_k'
+xₖ₊₁ = Axₖ + Buₖ
+```
+
+On Linux, this process can be streamlined further by adding the following Bash function to your .bashrc (requires `wl-clipboard` on Wayland or `xclip` on X11):
+```bash
+# Converts LaTeX to Unicode, prints the result, and copies it to the clipboard
+uc() {
+  if [ $WAYLAND_DISPLAY ]; then
+    python -m unicodeit.cli $@ | tee >(wl-copy -n)
+  else
+    python -m unicodeit.cli $@ | tee >(xclip -sel)
+  fi
+}
+```
+
+Here's example usage:
+```bash
+$ uc 'x_{k+1} = Ax_k + Bu_k'
+xₖ₊₁ = Axₖ + Buₖ
+```
+
 ## Submitting Changes
 
 ### Pull Request Format

README.md

@@ -40,7 +40,7 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen
 
 ## Requirements
 
-- [JDK 11](https://adoptopenjdk.net/)
+- [JDK 11](https://adoptium.net/temurin/releases/?version=11)
     - Note that the JRE is insufficient; the full JDK is required
     - On Ubuntu, run `sudo apt install openjdk-11-jdk`
     - On Windows, install the JDK 11 .msi from the link above
@@ -48,7 +48,7 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen
 - C++ compiler
     - On Linux, install GCC 11 or greater
     - On Windows, install [Visual Studio Community 2022](https://visualstudio.microsoft.com/vs/community/) and select the C++ programming language during installation (Gradle can't use the build tools for Visual Studio)
-    - On macOS, install the Xcode command-line build tools via `xcode-select --install`
+    - On macOS, install the Xcode command-line build tools via `xcode-select --install`. Xcode 13 or later is required.
 - ARM compiler toolchain
     - Run `./gradlew installRoboRioToolchain` after cloning this repository
     - If the WPILib installer was used, this toolchain is already installed

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
    */
@@ -187,7 +189,10 @@ public class AprilTagFieldLayout {
   }
 
   /**
-   * Deserializes a field layout from a resource within a jar file.
+   * Deserializes a field layout from a resource within a internal jar file.
+   *
+   * <p>Users should use {@link AprilTagFields#loadAprilTagLayoutField()} to load official layouts
+   * and {@link #AprilTagFieldLayout(String)} for custom layouts.
    *
    * @param resourcePath The absolute path of the resource
    * @return The deserialized layout

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

@@ -4,17 +4,30 @@
 
 package edu.wpi.first.apriltag;
 
+import java.io.IOException;
+
 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;
 
   AprilTagFields(String resourceFile) {
     m_resourceFile = kBaseResourceDir + resourceFile;
   }
+
+  /**
+   * Get a {@link AprilTagFieldLayout} from the resource JSON.
+   *
+   * @return AprilTagFieldLayout of the field
+   * @throws IOException If the layout does not exist
+   */
+  public AprilTagFieldLayout loadAprilTagLayoutField() throws IOException {
+    return AprilTagFieldLayout.loadFromResource(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/2022-rapidreact.json

@@ -1,415 +1,440 @@
 {
-  "tags" : [ {
-    "ID" : 0,
-    "pose" : {
-      "translation" : {
-        "x" : -0.0035306,
-        "y" : 7.578928199999999,
-        "z" : 0.8858503999999999
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 1.0,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 0.0
+  "tags": [
+    {
+      "ID": 0,
+      "pose": {
+        "translation": {
+          "x": -0.0035306,
+          "y": 7.578928199999999,
+          "z": 0.8858503999999999
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 3.2327088,
+          "y": 5.486654,
+          "z": 1.7254728
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 3.067812,
+          "y": 5.3305202,
+          "z": 1.3762228
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": -0.7071067811865475
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 0.0039878,
+          "y": 5.058536999999999,
+          "z": 0.80645
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 0.0039878,
+          "y": 3.5124898,
+          "z": 0.80645
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 0.12110719999999998,
+          "y": 1.7178274,
+          "z": 0.8906002000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9196502204050923,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.39273842708457407
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 0.8733027999999999,
+          "y": 0.9412985999999999,
+          "z": 0.8906002000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9196502204050923,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.39273842708457407
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 1.6150844,
+          "y": 0.15725139999999999,
+          "z": 0.8906002000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9196502204050923,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.39273842708457407
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 16.4627306,
+          "y": 0.6506718,
+          "z": 0.8858503999999999
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766E-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 13.2350002,
+          "y": 2.743454,
+          "z": 1.7254728
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766E-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 13.391388000000001,
+          "y": 2.8998418,
+          "z": 1.3762228
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865475
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 16.4552122,
+          "y": 3.1755079999999998,
+          "z": 0.80645
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766E-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 16.4552122,
+          "y": 4.7171356,
+          "z": 0.80645
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766E-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 16.3350194,
+          "y": 6.5149729999999995,
+          "z": 0.8937752
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.37298778257580906,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.9278362538989199
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 15.5904946,
+          "y": 7.292695599999999,
+          "z": 0.8906002000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.37298778257580906,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.9278362538989199
+          }
+        }
+      }
+    },
+    {
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 14.847188999999998,
+          "y": 8.0691228,
+          "z": 0.8906002000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.37298778257580906,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.9278362538989199
+          }
+        }
+      }
+    },
+    {
+      "ID": 40,
+      "pose": {
+        "translation": {
+          "x": 7.874127,
+          "y": 4.9131728,
+          "z": 0.7032752
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5446390350150271,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.838670567945424
+          }
+        }
+      }
+    },
+    {
+      "ID": 41,
+      "pose": {
+        "translation": {
+          "x": 7.4312271999999995,
+          "y": 3.759327,
+          "z": 0.7032752
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.20791169081775934,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.9781476007338057
+          }
+        }
+      }
+    },
+    {
+      "ID": 42,
+      "pose": {
+        "translation": {
+          "x": 8.585073,
+          "y": 3.3164272,
+          "z": 0.7032752
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.838670567945424,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": -0.5446390350150271
+          }
+        }
+      }
+    },
+    {
+      "ID": 43,
+      "pose": {
+        "translation": {
+          "x": 9.0279728,
+          "y": 4.470273,
+          "z": 0.7032752
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9781476007338057,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.20791169081775934
+          }
+        }
+      }
+    },
+    {
+      "ID": 50,
+      "pose": {
+        "translation": {
+          "x": 7.6790296,
+          "y": 4.3261534,
+          "z": 2.4177244
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.17729273396782605,
+            "X": -0.22744989571511945,
+            "Y": 0.04215534644161733,
+            "Z": 0.9565859910053995
+          }
+        }
+      }
+    },
+    {
+      "ID": 51,
+      "pose": {
+        "translation": {
+          "x": 8.0182466,
+          "y": 3.5642296,
+          "z": 2.4177244
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.5510435465842192,
+            "X": -0.19063969497246985,
+            "Y": -0.13102303230819815,
+            "Z": 0.8017733354717242
+          }
+        }
+      }
+    },
+    {
+      "ID": 52,
+      "pose": {
+        "translation": {
+          "x": 8.7801704,
+          "y": 3.9034466,
+          "z": 2.4177244
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.9565859910053994,
+            "X": -0.04215534644161739,
+            "Y": -0.22744989571511942,
+            "Z": 0.17729273396782633
+          }
+        }
+      }
+    },
+    {
+      "ID": 53,
+      "pose": {
+        "translation": {
+          "x": 8.4409534,
+          "y": 4.6653704,
+          "z": 2.4177244
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8017733354717241,
+            "X": -0.1310230323081982,
+            "Y": 0.19063969497246983,
+            "Z": 0.5510435465842194
+          }
         }
       }
     }
-  }, {
-    "ID" : 1,
-    "pose" : {
-      "translation" : {
-        "x" : 3.2327088,
-        "y" : 5.486654,
-        "z" : 1.7254728
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 1.0,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 0.0
-        }
-      }
-    }
-  }, {
-    "ID" : 2,
-    "pose" : {
-      "translation" : {
-        "x" : 3.067812,
-        "y" : 5.3305202,
-        "z" : 1.3762228
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 0.7071067811865476,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : -0.7071067811865475
-        }
-      }
-    }
-  }, {
-    "ID" : 3,
-    "pose" : {
-      "translation" : {
-        "x" : 0.0039878,
-        "y" : 5.058536999999999,
-        "z" : 0.80645
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 1.0,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 0.0
-        }
-      }
-    }
-  }, {
-    "ID" : 4,
-    "pose" : {
-      "translation" : {
-        "x" : 0.0039878,
-        "y" : 3.5124898,
-        "z" : 0.80645
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 1.0,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 0.0
-        }
-      }
-    }
-  }, {
-    "ID" : 5,
-    "pose" : {
-      "translation" : {
-        "x" : 0.12110719999999998,
-        "y" : 1.7178274,
-        "z" : 0.8906002000000001
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 0.9196502204050923,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 0.39273842708457407
-        }
-      }
-    }
-  }, {
-    "ID" : 6,
-    "pose" : {
-      "translation" : {
-        "x" : 0.8733027999999999,
-        "y" : 0.9412985999999999,
-        "z" : 0.8906002000000001
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 0.9196502204050923,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 0.39273842708457407
-        }
-      }
-    }
-  }, {
-    "ID" : 7,
-    "pose" : {
-      "translation" : {
-        "x" : 1.6150844,
-        "y" : 0.15725139999999999,
-        "z" : 0.8906002000000001
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 0.9196502204050923,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 0.39273842708457407
-        }
-      }
-    }
-  }, {
-    "ID" : 10,
-    "pose" : {
-      "translation" : {
-        "x" : 16.4627306,
-        "y" : 0.6506718,
-        "z" : 0.8858503999999999
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 6.123233995736766E-17,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 1.0
-        }
-      }
-    }
-  }, {
-    "ID" : 11,
-    "pose" : {
-      "translation" : {
-        "x" : 13.2350002,
-        "y" : 2.743454,
-        "z" : 1.7254728
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 6.123233995736766E-17,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 1.0
-        }
-      }
-    }
-  }, {
-    "ID" : 12,
-    "pose" : {
-      "translation" : {
-        "x" : 13.391388000000001,
-        "y" : 2.8998418,
-        "z" : 1.3762228
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 0.7071067811865476,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 0.7071067811865475
-        }
-      }
-    }
-  }, {
-    "ID" : 13,
-    "pose" : {
-      "translation" : {
-        "x" : 16.4552122,
-        "y" : 3.1755079999999998,
-        "z" : 0.80645
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 6.123233995736766E-17,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 1.0
-        }
-      }
-    }
-  }, {
-    "ID" : 14,
-    "pose" : {
-      "translation" : {
-        "x" : 16.4552122,
-        "y" : 4.7171356,
-        "z" : 0.80645
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 6.123233995736766E-17,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 1.0
-        }
-      }
-    }
-  }, {
-    "ID" : 15,
-    "pose" : {
-      "translation" : {
-        "x" : 16.3350194,
-        "y" : 6.5149729999999995,
-        "z" : 0.8937752
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : -0.37298778257580906,
-          "X" : -0.0,
-          "Y" : 0.0,
-          "Z" : 0.9278362538989199
-        }
-      }
-    }
-  }, {
-    "ID" : 16,
-    "pose" : {
-      "translation" : {
-        "x" : 15.5904946,
-        "y" : 7.292695599999999,
-        "z" : 0.8906002000000001
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : -0.37298778257580906,
-          "X" : -0.0,
-          "Y" : 0.0,
-          "Z" : 0.9278362538989199
-        }
-      }
-    }
-  }, {
-    "ID" : 17,
-    "pose" : {
-      "translation" : {
-        "x" : 14.847188999999998,
-        "y" : 8.0691228,
-        "z" : 0.8906002000000001
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : -0.37298778257580906,
-          "X" : -0.0,
-          "Y" : 0.0,
-          "Z" : 0.9278362538989199
-        }
-      }
-    }
-  }, {
-    "ID" : 40,
-    "pose" : {
-      "translation" : {
-        "x" : 7.874127,
-        "y" : 4.9131728,
-        "z" : 0.7032752
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 0.5446390350150271,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 0.838670567945424
-        }
-      }
-    }
-  }, {
-    "ID" : 41,
-    "pose" : {
-      "translation" : {
-        "x" : 7.4312271999999995,
-        "y" : 3.759327,
-        "z" : 0.7032752
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : -0.20791169081775934,
-          "X" : -0.0,
-          "Y" : 0.0,
-          "Z" : 0.9781476007338057
-        }
-      }
-    }
-  }, {
-    "ID" : 42,
-    "pose" : {
-      "translation" : {
-        "x" : 8.585073,
-        "y" : 3.3164272,
-        "z" : 0.7032752
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 0.838670567945424,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : -0.5446390350150271
-        }
-      }
-    }
-  }, {
-    "ID" : 43,
-    "pose" : {
-      "translation" : {
-        "x" : 9.0279728,
-        "y" : 4.470273,
-        "z" : 0.7032752
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 0.9781476007338057,
-          "X" : 0.0,
-          "Y" : 0.0,
-          "Z" : 0.20791169081775934
-        }
-      }
-    }
-  }, {
-    "ID" : 50,
-    "pose" : {
-      "translation" : {
-        "x" : 7.6790296,
-        "y" : 4.3261534,
-        "z" : 2.4177244
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 0.17729273396782605,
-          "X" : -0.22744989571511945,
-          "Y" : 0.04215534644161733,
-          "Z" : 0.9565859910053995
-        }
-      }
-    }
-  }, {
-    "ID" : 51,
-    "pose" : {
-      "translation" : {
-        "x" : 8.0182466,
-        "y" : 3.5642296,
-        "z" : 2.4177244
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : -0.5510435465842192,
-          "X" : -0.19063969497246985,
-          "Y" : -0.13102303230819815,
-          "Z" : 0.8017733354717242
-        }
-      }
-    }
-  }, {
-    "ID" : 52,
-    "pose" : {
-      "translation" : {
-        "x" : 8.7801704,
-        "y" : 3.9034466,
-        "z" : 2.4177244
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : -0.9565859910053994,
-          "X" : -0.04215534644161739,
-          "Y" : -0.22744989571511942,
-          "Z" : 0.17729273396782633
-        }
-      }
-    }
-  }, {
-    "ID" : 53,
-    "pose" : {
-      "translation" : {
-        "x" : 8.4409534,
-        "y" : 4.6653704,
-        "z" : 2.4177244
-      },
-      "rotation" : {
-        "quaternion" : {
-          "W" : 0.8017733354717241,
-          "X" : -0.1310230323081982,
-          "Y" : 0.19063969497246983,
-          "Z" : 0.5510435465842194
-        }
-      }
-    }
-  } ],
-  "field" : {
-    "length" : 16.4592,
-    "width" : 8.2296
+  ],
+  "field": {
+    "length": 16.4592,
+    "width": 8.2296
   }
 }

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/java/edu/wpi/first/apriltag/LoadConfigTest.java

@@ -23,16 +23,13 @@ class LoadConfigTest {
   @ParameterizedTest
   @EnumSource(AprilTagFields.class)
   void testLoad(AprilTagFields field) {
-    AprilTagFieldLayout layout =
-        Assertions.assertDoesNotThrow(
-            () -> AprilTagFieldLayout.loadFromResource(field.m_resourceFile));
+    AprilTagFieldLayout layout = Assertions.assertDoesNotThrow(field::loadAprilTagLayoutField);
     assertNotNull(layout);
   }
 
   @Test
   void test2022RapidReact() throws IOException {
-    AprilTagFieldLayout layout =
-        AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile);
+    AprilTagFieldLayout layout = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField();
 
     // Blue Hangar Truss - Hub
     Pose3d expectedPose =

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"
 }

cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp

@@ -474,6 +474,7 @@ cs::UsbCamera CameraServer::StartAutomaticCapture() {
 }
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(int dev) {
+  ::GetInstance();
   cs::UsbCamera camera{fmt::format("USB Camera {}", dev), dev};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -483,6 +484,7 @@ cs::UsbCamera CameraServer::StartAutomaticCapture(int dev) {
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
                                                   int dev) {
+  ::GetInstance();
   cs::UsbCamera camera{name, dev};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -492,6 +494,7 @@ cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
                                                   std::string_view path) {
+  ::GetInstance();
   cs::UsbCamera camera{name, path};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -517,6 +520,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::span<const std::string> hosts) {
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            std::string_view host) {
+  ::GetInstance();
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -526,6 +530,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            const char* host) {
+  ::GetInstance();
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -535,6 +540,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            const std::string& host) {
+  ::GetInstance();
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -544,6 +550,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            std::span<const std::string> hosts) {
+  ::GetInstance();
   cs::AxisCamera camera{name, hosts};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -552,10 +559,11 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 }
 
 cs::MjpegServer CameraServer::AddSwitchedCamera(std::string_view name) {
+  auto& inst = ::GetInstance();
   // create a dummy CvSource
   cs::CvSource source{name, cs::VideoMode::PixelFormat::kMJPEG, 160, 120, 30};
   cs::MjpegServer server = StartAutomaticCapture(source);
-  ::GetInstance().m_fixedSources[server.GetHandle()] = source.GetHandle();
+  inst.m_fixedSources[server.GetHandle()] = source.GetHandle();
 
   return server;
 }
@@ -632,6 +640,7 @@ cs::CvSink CameraServer::GetVideo(std::string_view name) {
 
 cs::CvSource CameraServer::PutVideo(std::string_view name, int width,
                                     int height) {
+  ::GetInstance();
   cs::CvSource source{name, cs::VideoMode::kMJPEG, width, height, 30};
   StartAutomaticCapture(source);
   return source;
@@ -648,6 +657,7 @@ cs::MjpegServer CameraServer::AddServer(std::string_view name) {
 }
 
 cs::MjpegServer CameraServer::AddServer(std::string_view name, int port) {
+  ::GetInstance();
   cs::MjpegServer server{name, port};
   AddServer(server);
   return server;

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

datalogtool/src/main/native/cpp/App.cpp

@@ -104,6 +104,8 @@ static void DisplayMainMenu() {
     ImGui::Text("v%s", GetWPILibVersion());
     ImGui::Separator();
     ImGui::Text("Save location: %s", glass::GetStorageDir().c_str());
+    ImGui::Text("%.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate,
+                ImGui::GetIO().Framerate);
     if (ImGui::Button("Close")) {
       ImGui::CloseCurrentPopup();
     }

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/2018-powerup.json

@@ -2,9 +2,18 @@
   "game": "FIRST Power Up",
   "field-image": "2018-field.jpg",
   "field-corners": {
-    "top-left": [125, 20],
-    "bottom-right": [827, 370]
+    "top-left": [
+      125,
+      20
+    ],
+    "bottom-right": [
+      827,
+      370
+    ]
   },
-  "field-size": [54, 27],
+  "field-size": [
+    54,
+    27
+  ],
   "field-unit": "feet"
 }

fieldImages/src/main/native/resources/edu/wpi/first/fields/2019-deepspace.json

@@ -1,10 +1,19 @@
 {
-"game" : "Destination: Deep Space",
-"field-image" : "2019-field.jpg",
-"field-corners": {
-	"top-left" : [217, 40],
-	"bottom-right" : [1372, 615]
-},
-"field-size" : [54, 27],
-"field-unit" : "foot"
+  "game": "Destination: Deep Space",
+  "field-image": "2019-field.jpg",
+  "field-corners": {
+    "top-left": [
+      217,
+      40
+    ],
+    "bottom-right": [
+      1372,
+      615
+    ]
+  },
+  "field-size": [
+    54,
+    27
+  ],
+  "field-unit": "foot"
 }

fieldImages/src/main/native/resources/edu/wpi/first/fields/2020-infiniterecharge.json

@@ -1,10 +1,19 @@
 {
-    "game" : "Infinite Recharge",
-    "field-image" : "2020-field.png",
-    "field-corners": {
-        "top-left" : [96, 25],
-        "bottom-right" : [1040, 514]
-    },
-    "field-size" : [52.4375, 26.9375],
-    "field-unit" : "foot"
-    }
+  "game": "Infinite Recharge",
+  "field-image": "2020-field.png",
+  "field-corners": {
+    "top-left": [
+      96,
+      25
+    ],
+    "bottom-right": [
+      1040,
+      514
+    ]
+  },
+  "field-size": [
+    52.4375,
+    26.9375
+  ],
+  "field-unit": "foot"
+}

fieldImages/src/main/native/resources/edu/wpi/first/fields/2021-barrelracingpath.json

@@ -1,10 +1,19 @@
 {
-    "game": "Barrel Racing Path",
-    "field-image": "2021-barrel.png",
-    "field-corners": {
-      "top-left": [20, 20],
-      "bottom-right": [780, 400]
-    },
-    "field-size": [30, 15],
-    "field-unit": "feet"
-  }
+  "game": "Barrel Racing Path",
+  "field-image": "2021-barrel.png",
+  "field-corners": {
+    "top-left": [
+      20,
+      20
+    ],
+    "bottom-right": [
+      780,
+      400
+    ]
+  },
+  "field-size": [
+    30,
+    15
+  ],
+  "field-unit": "feet"
+}

fieldImages/src/main/native/resources/edu/wpi/first/fields/2021-bouncepath.json

@@ -1,10 +1,19 @@
 {
-    "game": "Bounce Path",
-    "field-image": "2021-bounce.png",
-    "field-corners": {
-      "top-left": [20, 20],
-      "bottom-right": [780, 400]
-    },
-    "field-size": [30, 15],
-    "field-unit": "feet"
-  }
+  "game": "Bounce Path",
+  "field-image": "2021-bounce.png",
+  "field-corners": {
+    "top-left": [
+      20,
+      20
+    ],
+    "bottom-right": [
+      780,
+      400
+    ]
+  },
+  "field-size": [
+    30,
+    15
+  ],
+  "field-unit": "feet"
+}

fieldImages/src/main/native/resources/edu/wpi/first/fields/2021-galacticsearcha.json

@@ -1,10 +1,19 @@
 {
-    "game": "Galactic Search A",
-    "field-image": "2021-galacticsearcha.png",
-    "field-corners": {
-      "top-left": [20, 20],
-      "bottom-right": [780, 400]
-    },
-    "field-size": [30, 15],
-    "field-unit": "feet"
-  }
+  "game": "Galactic Search A",
+  "field-image": "2021-galacticsearcha.png",
+  "field-corners": {
+    "top-left": [
+      20,
+      20
+    ],
+    "bottom-right": [
+      780,
+      400
+    ]
+  },
+  "field-size": [
+    30,
+    15
+  ],
+  "field-unit": "feet"
+}

fieldImages/src/main/native/resources/edu/wpi/first/fields/2021-galacticsearchb.json

@@ -1,10 +1,19 @@
 {
-    "game": "Galactic Search B",
-    "field-image": "2021-galacticsearchb.png",
-    "field-corners": {
-      "top-left": [20, 20],
-      "bottom-right": [780, 400]
-    },
-    "field-size": [30, 15],
-    "field-unit": "feet"
-  }
+  "game": "Galactic Search B",
+  "field-image": "2021-galacticsearchb.png",
+  "field-corners": {
+    "top-left": [
+      20,
+      20
+    ],
+    "bottom-right": [
+      780,
+      400
+    ]
+  },
+  "field-size": [
+    30,
+    15
+  ],
+  "field-unit": "feet"
+}

fieldImages/src/main/native/resources/edu/wpi/first/fields/2021-infiniterecharge.json

@@ -1,10 +1,19 @@
 {
-    "game": "Infinite Recharge 2021",
-    "field-image": "2021-field.png",
-    "field-corners": {
-      "top-left": [127, 34],
-      "bottom-right": [1323, 649]
-    },
-    "field-size": [52.4375, 26.9375],
-    "field-unit": "foot"
-  }
+  "game": "Infinite Recharge 2021",
+  "field-image": "2021-field.png",
+  "field-corners": {
+    "top-left": [
+      127,
+      34
+    ],
+    "bottom-right": [
+      1323,
+      649
+    ]
+  },
+  "field-size": [
+    52.4375,
+    26.9375
+  ],
+  "field-unit": "foot"
+}

fieldImages/src/main/native/resources/edu/wpi/first/fields/2021-slalompath.json

@@ -1,10 +1,19 @@
 {
-    "game": "Slalom Path",
-    "field-image": "2021-slalom.png",
-    "field-corners": {
-      "top-left": [20, 20],
-      "bottom-right": [780, 400]
-    },
-    "field-size": [30, 15],
-    "field-unit": "feet"
-  }
+  "game": "Slalom Path",
+  "field-image": "2021-slalom.png",
+  "field-corners": {
+    "top-left": [
+      20,
+      20
+    ],
+    "bottom-right": [
+      780,
+      400
+    ]
+  },
+  "field-size": [
+    30,
+    15
+  ],
+  "field-unit": "feet"
+}

fieldImages/src/main/native/resources/edu/wpi/first/fields/2022-rapidreact.json

@@ -1,10 +1,19 @@
 {
-    "game": "Rapid React",
-    "field-image": "2022-field.png",
-    "field-corners": {
-      "top-left": [74, 50],
-      "bottom-right": [1774, 900]
-    },
-    "field-size": [54, 27],
-    "field-unit": "foot"
-  }
+  "game": "Rapid React",
+  "field-image": "2022-field.png",
+  "field-corners": {
+    "top-left": [
+      74,
+      50
+    ],
+    "bottom-right": [
+      1774,
+      900
+    ]
+  },
+  "field-size": [
+    54,
+    27
+  ],
+  "field-unit": "foot"
+}

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

@@ -0,0 +1,19 @@
+{
+  "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/app/native/cpp/main.cpp

@@ -52,6 +52,7 @@ static bool gSetEnterKey = false;
 static bool gKeyEdit = false;
 static int* gEnterKey;
 static void (*gPrevKeyCallback)(GLFWwindow*, int, int, int, int);
+static bool gNetworkTablesDebugLog = false;
 
 static void RemapEnterKeyCallback(GLFWwindow* window, int key, int scancode,
                                   int action, int mods) {
@@ -72,9 +73,8 @@ static void RemapEnterKeyCallback(GLFWwindow* window, int key, int scancode,
 static void NtInitialize() {
   auto inst = nt::GetDefaultInstance();
   auto poller = nt::CreateListenerPoller(inst);
-  nt::AddPolledListener(
-      poller, inst,
-      NT_EVENT_CONNECTION | NT_EVENT_IMMEDIATE | NT_EVENT_LOGMESSAGE);
+  nt::AddPolledListener(poller, inst, NT_EVENT_CONNECTION | NT_EVENT_IMMEDIATE);
+  nt::AddPolledLogger(poller, 0, 100);
   gui::AddEarlyExecute([poller] {
     auto win = gui::GetSystemWindow();
     if (!win) {
@@ -98,6 +98,8 @@ static void NtInitialize() {
           level = "ERROR: ";
         } else if (msg->level >= NT_LOG_WARNING) {
           level = "WARNING: ";
+        } else if (msg->level < NT_LOG_INFO && !gNetworkTablesDebugLog) {
+          continue;
         }
         gNetworkTablesLog.Append(fmt::format(
             "{}{} ({}:{})\n", level, msg->message, msg->filename, msg->line));
@@ -232,6 +234,8 @@ int main(int argc, char** argv) {
       if (gNetworkTablesLogWindow) {
         gNetworkTablesLogWindow->DisplayMenuItem("NetworkTables Log");
       }
+      ImGui::MenuItem("NetworkTables Debug Logging", nullptr,
+                      &gNetworkTablesDebugLog);
       ImGui::Separator();
       gNtProvider->DisplayMenu();
       ImGui::EndMenu();
@@ -265,6 +269,8 @@ int main(int argc, char** argv) {
       ImGui::Text("v%s", GetWPILibVersion());
       ImGui::Separator();
       ImGui::Text("Save location: %s", glass::GetStorageDir().c_str());
+      ImGui::Text("%.3f ms/frame (%.1f FPS)",
+                  1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate);
       if (ImGui::Button("Close")) {
         ImGui::CloseCurrentPopup();
       }

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} {