[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/cmake.yml

@@ -12,7 +12,7 @@ jobs:
       fail-fast: false
       matrix:
         include:
-          - os: ubuntu-latest
+          - os: ubuntu-22.04
             name: Linux
             container: wpilib/roborio-cross-ubuntu:2023-22.04
             flags: ""

.github/workflows/comment-command.yml

@@ -4,9 +4,9 @@ on:
     types: [ created ]
 
 jobs:
-  wpiformat:
-    if: github.event.issue.pull_request && startsWith(github.event.comment.body, '/wpiformat')
-    runs-on: ubuntu-latest
+  format:
+    if: github.event.issue.pull_request && startsWith(github.event.comment.body, '/format')
+    runs-on: ubuntu-22.04
     steps:
       - name: React Rocket
         uses: actions/github-script@v4
@@ -37,21 +37,28 @@ jobs:
         uses: actions/setup-python@v2
         with:
           python-version: 3.8
+      - name: Setup Java
+        uses: actions/setup-java@v3
+        with:
+          distribution: 'zulu'
+          java-version: 11
       - name: Install clang-format
         run: |
           wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add -
-          sudo sh -c "echo 'deb http://apt.llvm.org/focal/ llvm-toolchain-focal-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
+          sudo sh -c "echo 'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
           sudo apt-get update -q
           sudo apt-get install -y clang-format-14
       - name: Install wpiformat
         run: pip3 install wpiformat
       - name: Run wpiformat
         run: wpiformat -clang 14
+      - name: Run spotlessApply
+        run: ./gradlew spotlessApply
       - name: Commit
         run: |
           # Set credentials
           git config user.name "github-actions[bot]"
           git config user.email "41898282+github-actions[bot]@users.noreply.github.com"
           # Commit
-          git commit -am "wpiformat"
+          git commit -am "Formatting fixes"
           git push

.github/workflows/documentation.yml

@@ -12,7 +12,7 @@ env:
 jobs:
   publish:
     name: "Documentation - Publish"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     if: github.repository_owner == 'wpilibsuite' && (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
     concurrency: ci-docs-publish
     steps:
@@ -24,8 +24,6 @@ jobs:
         with:
           distribution: 'zulu'
           java-version: 13
-      - name: Install libclang-9
-        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
       - name: Set environment variables (Development)
         run: |
           echo "TARGET_FOLDER=$BASE_PATH/development" >> $GITHUB_ENV

.github/workflows/gazebo.yml

@@ -1,19 +0,0 @@
-name: Gazebo
-
-on: [pull_request, push]
-
-concurrency:
-  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
-  cancel-in-progress: true
-
-jobs:
-  build:
-    name: "Build"
-    runs-on: ubuntu-latest
-    container: wpilib/gazebo-ubuntu:22.04
-    steps:
-      - uses: actions/checkout@v3
-        with:
-          fetch-depth: 0
-      - name: Build with Gradle
-        run: ./gradlew simulation:frc_gazebo_plugins:build simulation:halsim_gazebo:build -PbuildServer -PforceGazebo

.github/workflows/gradle.yml

@@ -25,7 +25,7 @@ jobs:
             artifact-name: Linux
             build-options: "-Ponlylinuxx86-64"
     name: "Build - ${{ matrix.artifact-name }}"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     container: ${{ matrix.container }}
     steps:
       - uses: actions/checkout@v3
@@ -46,7 +46,7 @@ jobs:
 
   build-host:
     env:
-      MACOSX_DEPLOYMENT_TARGET: 10.14
+      MACOSX_DEPLOYMENT_TARGET: 10.15
     strategy:
       fail-fast: false
       matrix:
@@ -66,7 +66,6 @@ jobs:
           - os: macOS-11
             artifact-name: macOS
             architecture: x64
-            build-options: "-Pbuildalldesktop"
             task: "build"
             outputs: "build/allOutputs"
           - os: windows-2022
@@ -83,7 +82,7 @@ jobs:
       - uses: actions/setup-java@v3
         with:
           distribution: 'zulu'
-          java-version: 11
+          java-version: 17
           architecture: ${{ matrix.architecture }}
       - name: Import Developer ID Certificate
         uses: wpilibsuite/import-signing-certificate@v1
@@ -112,7 +111,7 @@ jobs:
           ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
           ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
       - name: Sign Libraries with Developer ID
-        run: ./gradlew build -PbuildServer -PskipJavaFormat -PdeveloperID=${{ secrets.APPLE_DEVELOPER_ID }} ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
+        run: ./gradlew copyAllOutputs --build-cache -PbuildServer -PskipJavaFormat -PdeveloperID=${{ secrets.APPLE_DEVELOPER_ID }} ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
         if: |
           matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
           (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v')))
@@ -123,7 +122,7 @@ jobs:
 
   build-documentation:
     name: "Build - Documentation"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
         with:
@@ -132,8 +131,6 @@ jobs:
         with:
           distribution: 'zulu'
           java-version: 13
-      - name: Install libclang-9
-        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
       - name: Set release environment variable
         run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
         if: startsWith(github.ref, 'refs/tags/v')
@@ -150,7 +147,7 @@ jobs:
   combine:
     name: Combine
     needs: [build-docker, build-host, build-documentation]
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
         with:

.github/workflows/lint-format.yml

@@ -13,7 +13,7 @@ concurrency:
 jobs:
   wpiformat:
     name: "wpiformat"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
       - name: Fetch all history and metadata
@@ -29,7 +29,7 @@ jobs:
       - name: Install clang-format
         run: |
           wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add -
-          sudo sh -c "echo 'deb http://apt.llvm.org/focal/ llvm-toolchain-focal-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
+          sudo sh -c "echo 'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
           sudo apt-get update -q
           sudo apt-get install -y clang-format-14
       - name: Install wpiformat
@@ -56,7 +56,7 @@ jobs:
 
   tidy:
     name: "clang-tidy"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     container: wpilib/roborio-cross-ubuntu:2023-22.04
     steps:
       - uses: actions/checkout@v3
@@ -73,7 +73,7 @@ jobs:
       - name: Install clang-tidy
         run: |
           wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add -
-          sudo sh -c "echo 'deb http://apt.llvm.org/focal/ llvm-toolchain-focal-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
+          sudo sh -c "echo 'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
           sudo apt-get update -q
           sudo apt-get install -y clang-tidy-14 clang-format-14
       - name: Install wpiformat
@@ -86,7 +86,7 @@ jobs:
         run: wpiformat -clang 14 -no-format -tidy-changed -compile-commands=build/compile_commands/linuxx86-64 -vv
   javaformat:
     name: "Java format"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     container: wpilib/ubuntu-base:22.04
     steps:
       - uses: actions/checkout@v3
@@ -105,7 +105,7 @@ jobs:
         if: ${{ failure() }}
   documentation:
     name: "Documentation"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
         with:
@@ -114,7 +114,5 @@ jobs:
         with:
           distribution: 'zulu'
           java-version: 13
-      - name: Install libclang-9
-        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
       - name: Build with Gradle
         run: ./gradlew docs:zipDocs -PbuildServer -PdocWarningsAsErrors ${{ env.EXTRA_GRADLE_ARGS }}

.github/workflows/sanitizers.yml

@@ -25,7 +25,7 @@ jobs:
             ctest-env: ""
             ctest-flags: ""
     name: "${{ matrix.name }}"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     container: wpilib/roborio-cross-ubuntu:2023-22.04
     steps:
       - uses: actions/checkout@v3

.github/workflows/upstream-utils.yml

@@ -13,7 +13,7 @@ concurrency:
 jobs:
   update:
     name: "Update"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
       - name: Fetch all history and metadata
@@ -49,6 +49,10 @@ jobs:
         run: |
           cd upstream_utils
           ./update_llvm.py
+      - name: Run update_mpack.py
+        run: |
+          cd upstream_utils
+          ./update_mpack.py
       - name: Run update_stack_walker.py
         run: |
           cd upstream_utils

.gitignore

@@ -5,6 +5,12 @@ doxygen.log
 build*/
 !buildSrc/
 
+simgui-ds.json
+simgui-window.json
+simgui.json
+
+networktables.json
+
 # Created by the jenkins test script
 test-reports
 
@@ -15,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 {

CMakeLists.txt

@@ -315,6 +315,7 @@ endif()
 
 if (WITH_WPILIB)
     set(WPILIBC_DEP_REPLACE ${WPILIBC_DEP_REPLACE_IMPL})
+    add_subdirectory(apriltag)
     add_subdirectory(wpilibj)
     add_subdirectory(wpilibc)
     add_subdirectory(wpilibNewCommands)

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

DevelopmentBuilds.md

@@ -8,7 +8,7 @@ This article contains instructions on building projects using a development buil
 
 Development builds are the per-commit build hosted every time a commit is pushed to the [allwpilib](https://github.com/wpilibsuite/allwpilib/) repository. These builds are then hosted on [artifactory](https://frcmaven.wpi.edu/artifactory/webapp/#/home).
 
-In order to build a project using a development build, find the build.gradle file and open it. Then, add the following code below the plugin section and replace YEAR with the year of the development version. It is also necessary to use a 2023 GradleRIO version, ie `2023.0.0-alpha-1`
+To build a project using a development build, find the build.gradle file and open it. Then, add the following code below the plugin section and replace YEAR with the year of the development version. It is also necessary to use a 2023 GradleRIO version, ie `2023.0.0-alpha-1`
 
 ```groovy
 wpi.maven.useLocal = false
@@ -46,6 +46,11 @@ wpi.versions.wpilibVersion = '2023.+'
 wpi.versions.wpimathVersion = '2023.+'
 ```
 
+### Development Build Documentation
+
+* C++: https://github.wpilib.org/allwpilib/docs/development/cpp/
+* Java: https://github.wpilib.org/allwpilib/docs/development/java/
+
 ## Local Build
 
 Building with a local build is very similar to building with a development build. Ensure you have built and published WPILib by following the instructions attached [here](https://github.com/wpilibsuite/allwpilib#building-wpilib). Next, find the ``build.gradle`` file in your robot project and open it. Then, add the following code below the plugin section and replace ``YEAR`` with the year of the local version.

MavenArtifacts.md

@@ -72,12 +72,16 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
 * wpigui
   * imgui
 
-* ntcore
-  * wpiutil
-
 * wpimath
   * wpiutil
 
+* wpinet
+  * wpiutil
+
+* ntcore
+  * wpiutil
+  * wpinet
+
 * glass/libglass
   * wpiutil
   * wpimath
@@ -85,6 +89,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
 
 * glass/libglassnt
   * wpiutil
+  * wpinet
   * ntcore
   * wpimath
   * wpigui
@@ -94,6 +99,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
 
 * halsim
   * wpiutil
+  * wpinet
   * ntcore
   * wpimath
   * wpigui
@@ -102,12 +108,14 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
 
 * cscore
   * opencv
+  * wpinet
   * wpiutil
 
 * cameraserver
   * ntcore
   * cscore
   * opencv
+  * wpinet
   * wpiutil
 
 * wpilibj
@@ -115,6 +123,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * cameraserver
   * ntcore
   * cscore
+  * wpinet
   * wpiutil
 
 * wpilibc
@@ -123,6 +132,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * ntcore
   * cscore
   * wpimath
+  * wpinet
   * wpiutil
 
 * wpilibNewCommands
@@ -132,6 +142,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * ntcore
   * cscore
   * wpimath
+  * wpinet
   * wpiutil
 
 

README.md

@@ -1,8 +1,8 @@
 # WPILib Project
 
-![CI](https://github.com/wpilibsuite/allwpilib/workflows/CI/badge.svg)
-[![C++ Documentation](https://img.shields.io/badge/documentation-c%2B%2B-blue)](https://first.wpi.edu/wpilib/allwpilib/docs/development/cpp/)
-[![Java Documentation](https://img.shields.io/badge/documentation-java-orange)](https://first.wpi.edu/wpilib/allwpilib/docs/development/java/)
+[![Gradle](https://github.com/wpilibsuite/allwpilib/actions/workflows/gradle.yml/badge.svg?branch=main)](https://github.com/wpilibsuite/allwpilib/actions/workflows/gradle.yml)
+[![C++ Documentation](https://img.shields.io/badge/documentation-c%2B%2B-blue)](https://github.wpilib.org/allwpilib/docs/development/cpp/)
+[![Java Documentation](https://img.shields.io/badge/documentation-java-orange)](https://github.wpilib.org/allwpilib/docs/development/java/)
 
 Welcome to the WPILib project. This repository contains the HAL, WPILibJ, and WPILibC projects. These are the core libraries for creating robot programs for the roboRIO.
 
@@ -15,7 +15,6 @@ Welcome to the WPILib project. This repository contains the HAL, WPILibJ, and WP
     - [Faster builds](#faster-builds)
     - [Using Development Builds](#using-development-builds)
     - [Custom toolchain location](#custom-toolchain-location)
-    - [Gazebo simulation](#gazebo-simulation)
     - [Formatting/Linting](#formattinglinting)
     - [CMake](#cmake)
   - [Publishing](#publishing)
@@ -33,7 +32,7 @@ Below is a list of instructions that guide you through cloning, building, publis
 1. Clone the repository with `git clone https://github.com/wpilibsuite/allwpilib.git`
 2. Build the repository with `./gradlew build` or `./gradlew build --build-cache` if you have an internet connection
 3. Publish the artifacts locally by running `./gradlew publish`
-4. [Update your](OtherVersions.md) `build.gradle` [to use the artifacts](OtherVersions.md)
+4. [Update your](DevelopmentBuilds.md) `build.gradle` [to use the artifacts](DevelopmentBuilds.md)
 
 # Building WPILib
 
@@ -41,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
@@ -49,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
@@ -104,7 +103,7 @@ Run with `--build-cache` on the command-line to use the shared [build cache](htt
 
 ### Using Development Builds
 
-Please read the documentation available [here](OtherVersions.md)
+Please read the documentation available [here](DevelopmentBuilds.md)
 
 ### Custom toolchain location
 
@@ -114,33 +113,14 @@ If you have installed the FRC Toolchain to a directory other than the default, o
 ./gradlew build -PtoolChainPath=some/path/to/frc/toolchain/bin
 ```
 
-### Gazebo simulation
-
-If you also want to force building Gazebo simulation support, add -PforceGazebo. This requires gazebo_transport. We have tested on 14.04 and 15.05, but any correct install of Gazebo should work, even on Windows if you build Gazebo from source. Correct means CMake needs to be able to find gazebo-config.cmake. See [The Gazebo website](https://gazebosim.org/) for installation instructions.
-
-```bash
-./gradlew build -PforceGazebo
-```
-
-If you prefer to use CMake directly, the you can still do so.
-The common CMake tasks are wpilibcSim, frc_gazebo_plugins, and gz_msgs
-
-```bash
-mkdir build #run this in the root of allwpilib
-cd build
-cmake ..
-make
-```
-
-
 ### Formatting/linting
 
+Once a PR has been submitted, formatting can be run in CI by commenting `/format` on the PR. A new commit will be pushed with the formatting changes.
+
 #### wpiformat
 
 wpiformat can be executed anywhere in the repository via `py -3 -m wpiformat -clang 14` on Windows or `python3 -m wpiformat -clang 14` on other platforms.
 
-Once a PR has been submitted, formatting can be run in CI by commenting `/wpiformat` on the PR. A new commit will be pushed with the formatting changes.
-
 #### Java Code Quality Tools
 
 The Java code quality tools Checkstyle, PMD, and Spotless can be run via `./gradlew javaFormat`. SpotBugs can be run via the `spotbugsMain`, `spotbugsTest`, and `spotbugsDev` tasks. These tools will all be run automatically by the `build` task. To disable this behavior, pass the `-PskipJavaFormat` flag.
@@ -164,9 +144,7 @@ The maven artifacts are described in [MavenArtifacts.md](MavenArtifacts.md)
 
 ## Structure and Organization
 
-The main WPILib code you're probably looking for is in WPILibJ and WPILibC. Those directories are split into shared, sim, and athena. Athena contains the WPILib code meant to run on your roboRIO. Sim is WPILib code meant to run on your computer with Gazebo, and shared is code shared between the two. Shared code must be platform-independent, since it will be compiled with both the ARM cross-compiler and whatever desktop compiler you are using (g++, msvc, etc...).
-
-The Simulation directory contains extra simulation tools and libraries, such as gz_msgs and JavaGazebo. See sub-directories for more information.
+The main WPILib code you're probably looking for is in WPILibJ and WPILibC. Those directories are split into shared, sim, and athena. Athena contains the WPILib code meant to run on your roboRIO. Sim is WPILib code meant to run on your computer, and shared is code shared between the two. Shared code must be platform-independent, since it will be compiled with both the ARM cross-compiler and whatever desktop compiler you are using (g++, msvc, etc...).
 
 The integration test directories for C++ and Java contain test code that runs on our test-system. When you submit code for review, it is tested by those programs. If you add new functionality you should make sure to write tests for it so we don't break it in the future.
 

apriltag/CMakeLists.txt

@@ -0,0 +1,118 @@
+project(apriltag)
+
+include(CompileWarnings)
+include(GenResources)
+include(FetchContent)
+
+FetchContent_Declare(
+    apriltaglib
+    GIT_REPOSITORY  https://github.com/wpilibsuite/apriltag.git
+    GIT_TAG         ad31e33d20f9782b7239cb15cde57c56c91383ad
+)
+
+# Don't use apriltag's CMakeLists.txt due to conflicting naming and JNI
+FetchContent_GetProperties(apriltaglib)
+if(NOT apriltaglib_POPULATED)
+    FetchContent_Populate(apriltaglib)
+endif()
+
+aux_source_directory(${apriltaglib_SOURCE_DIR}/common APRILTAGLIB_COMMON_SRC)
+file(GLOB TAG_FILES ${apriltaglib_SOURCE_DIR}/tag*.c)
+set(APRILTAGLIB_SRCS ${apriltaglib_SOURCE_DIR}/apriltag.c ${apriltaglib_SOURCE_DIR}/apriltag_pose.c ${apriltaglib_SOURCE_DIR}/apriltag_quad_thresh.c)
+
+file(GLOB apriltag_jni_src src/main/native/cpp/jni/AprilTagJNI.cpp)
+
+if (WITH_JAVA)
+  find_package(Java REQUIRED)
+  find_package(JNI REQUIRED)
+  include(UseJava)
+  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+
+  set(CMAKE_JNI_TARGET true)
+
+  file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
+  file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+  find_file(OPENCV_JAR_FILE
+    NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar
+    PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/share/java
+    NO_DEFAULT_PATH)
+
+  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 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}
+    INCLUDE_JARS wpimath_jar ${EJML_JARS} wpiutil_jar ${OPENCV_JAR_FILE}
+    OUTPUT_NAME apriltag
+    GENERATE_NATIVE_HEADERS apriltag_jni_headers)
+
+  get_property(APRILTAG_JAR_FILE TARGET apriltag_jar PROPERTY JAR_FILE)
+  install(FILES ${APRILTAG_JAR_FILE} DESTINATION "${java_lib_dest}")
+
+  set_property(TARGET apriltag_jar PROPERTY FOLDER "java")
+
+  add_library(apriltagjni ${apriltag_jni_src})
+  wpilib_target_warnings(apriltagjni)
+  target_link_libraries(apriltagjni PUBLIC apriltag)
+
+  set_property(TARGET apriltagjni PROPERTY FOLDER "libraries")
+
+  target_link_libraries(apriltagjni PRIVATE apriltag_jni_headers)
+  add_dependencies(apriltagjni apriltag_jar)
+
+  if (MSVC)
+    install(TARGETS apriltagjni RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
+  endif()
+
+  install(TARGETS apriltagjni EXPORT apriltagjni DESTINATION "${main_lib_dest}")
+
+endif()
+
+GENERATE_RESOURCES(src/main/native/resources/edu/wpi/first/apriltag generated/main/cpp APRILTAG frc apriltag_resources_src)
+
+file(GLOB apriltag_native_src src/main/native/cpp/*.cpp)
+
+add_library(apriltag ${apriltag_native_src} ${apriltag_resources_src} ${APRILTAGLIB_SRCS} ${APRILTAGLIB_COMMON_SRC} ${TAG_FILES})
+set_target_properties(apriltag PROPERTIES DEBUG_POSTFIX "d")
+
+set_property(TARGET apriltag PROPERTY FOLDER "libraries")
+target_compile_features(apriltag PUBLIC cxx_std_20)
+wpilib_target_warnings(apriltag)
+# disable warnings that apriltaglib can't handle
+if (MSVC)
+  target_compile_options(apriltag PRIVATE /wd4018)
+else()
+  target_compile_options(apriltag PRIVATE -Wno-sign-compare -Wno-gnu-zero-variadic-macro-arguments)
+endif()
+
+target_link_libraries(apriltag wpimath)
+
+target_include_directories(apriltag PUBLIC
+  $<BUILD_INTERFACE:${apriltaglib_SOURCE_DIR}>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+  $<INSTALL_INTERFACE:${include_dest}/apriltag>)
+
+install(TARGETS apriltag EXPORT apriltag DESTINATION "${main_lib_dest}")
+install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/apriltag")
+
+if (WITH_JAVA AND MSVC)
+    install(TARGETS apriltag RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
+endif()
+
+if (WITH_FLAT_INSTALL)
+    set (apriltag_config_dir ${wpilib_dest})
+else()
+    set (apriltag_config_dir share/apriltag)
+endif()
+
+configure_file(apriltag-config.cmake.in ${WPILIB_BINARY_DIR}/apriltag-config.cmake )
+install(FILES ${WPILIB_BINARY_DIR}/apriltag-config.cmake DESTINATION ${apriltag_config_dir})
+install(EXPORT apriltag DESTINATION ${apriltag_config_dir})
+
+if (WITH_TESTS)
+    wpilib_add_test(apriltag src/test/native/cpp)
+    target_include_directories(apriltag_test PRIVATE src/test/native/include)
+    target_link_libraries(apriltag_test apriltag gmock_main)
+endif()

apriltag/apriltag-config.cmake.in

@@ -0,0 +1,7 @@
+include(CMakeFindDependencyMacro)
+@FILENAME_DEP_REPLACE@
+@WPIMATH_DEP_REPLACE@
+@WPIUTIL_DEP_REPLACE@
+
+@FILENAME_DEP_REPLACE@
+include(${SELF_DIR}/apriltag.cmake)

apriltag/build.gradle

@@ -0,0 +1,88 @@
+apply from: "${rootDir}/shared/resources.gradle"
+
+ext {
+    nativeName = 'apriltag'
+    devMain = 'edu.wpi.first.apriltag.DevMain'
+    useJava = true
+    useCpp = true
+    sharedCvConfigs = [
+        apriltagDev : [],
+        apriltagTest: []]
+    staticCvConfigs = []
+
+    def generateTask = createGenerateResourcesTask('main', 'APRILTAG', 'frc', project)
+
+    tasks.withType(CppCompile) {
+        dependsOn generateTask
+    }
+    splitSetup = {
+        it.sources {
+            resourcesCpp(CppSourceSet) {
+                source {
+                    srcDirs "$buildDir/generated/main/cpp", "$rootDir/shared/singlelib"
+                    include '*.cpp'
+                }
+            }
+        }
+    }
+}
+
+evaluationDependsOn(':wpimath')
+
+apply from: "${rootDir}/shared/jni/setupBuild.gradle"
+apply from: "${rootDir}/shared/apriltaglib.gradle"
+apply from: "${rootDir}/shared/opencv.gradle"
+
+dependencies {
+    implementation project(':wpimath')
+    devImplementation project(':wpimath')
+}
+
+sourceSets {
+    main {
+        resources {
+            srcDirs 'src/main/native/resources'
+        }
+    }
+}
+
+model {
+    components {}
+    binaries {
+        all {
+            if (!it.buildable || !(it instanceof NativeBinarySpec)) {
+                return
+            }
+            it.cppCompiler.define 'WPILIB_EXPORTS'
+
+            if (it.component.name == "${nativeName}JNI") {
+                lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+            } else {
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            }
+
+            nativeUtils.useRequiredLibrary(it, 'apriltaglib')
+        }
+    }
+    tasks {
+        def c = $.components
+        def found = false
+        def systemArch = getCurrentArch()
+        c.each {
+            if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") {
+                it.binaries.each {
+                    if (!found) {
+                        def arch = it.targetPlatform.name
+                        if (arch == systemArch) {
+                            def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
+
+                            found = true
+                        }
+                    }
+                }
+            }
+        }
+    }
+}

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

@@ -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.
+
+package edu.wpi.first.apriltag;
+
+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

@@ -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.
+
+#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/AprilTag.java

@@ -0,0 +1,47 @@
+// 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 com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.geometry.Pose3d;
+import java.util.Objects;
+
+@SuppressWarnings("MemberName")
+public class AprilTag {
+  @JsonProperty(value = "ID")
+  public int ID;
+
+  @JsonProperty(value = "pose")
+  public Pose3d pose;
+
+  @SuppressWarnings("ParameterName")
+  @JsonCreator
+  public AprilTag(
+      @JsonProperty(required = true, value = "ID") int ID,
+      @JsonProperty(required = true, value = "pose") Pose3d pose) {
+    this.ID = ID;
+    this.pose = pose;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof AprilTag) {
+      var other = (AprilTag) obj;
+      return ID == other.ID && pose.equals(other.pose);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(ID, pose);
+  }
+
+  @Override
+  public String toString() {
+    return "AprilTag(ID: " + ID + ", pose: " + pose + ")";
+  }
+}

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

@@ -0,0 +1,241 @@
+// 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 com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnore;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.io.IOException;
+import java.io.InputStream;
+import java.io.InputStreamReader;
+import java.nio.file.Path;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.Objects;
+import java.util.Optional;
+
+/**
+ * Class for representing a layout of AprilTags on a field and reading them from a JSON format.
+ *
+ * <p>The JSON format contains two top-level objects, "tags" and "field". The "tags" object is a
+ * list of all AprilTags contained within a layout. Each AprilTag serializes to a JSON object
+ * containing an ID and a Pose3d. The "field" object is a descriptor of the size of the field in
+ * meters with "width" and "length" values. This is to account for arbitrary field sizes when
+ * transforming the poses.
+ *
+ * <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)
+public class AprilTagFieldLayout {
+  public enum OriginPosition {
+    kBlueAllianceWallRightSide,
+    kRedAllianceWallRightSide,
+  }
+
+  private final Map<Integer, AprilTag> m_apriltags = new HashMap<>();
+
+  @JsonProperty(value = "field")
+  private FieldDimensions m_fieldDimensions;
+
+  private Pose3d m_origin;
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   * @throws IOException If reading from the file fails.
+   */
+  public AprilTagFieldLayout(String path) throws IOException {
+    this(Path.of(path));
+  }
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   * @throws IOException If reading from the file fails.
+   */
+  public AprilTagFieldLayout(Path path) throws IOException {
+    AprilTagFieldLayout layout =
+        new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class);
+    m_apriltags.putAll(layout.m_apriltags);
+    m_fieldDimensions = layout.m_fieldDimensions;
+    setOrigin(OriginPosition.kBlueAllianceWallRightSide);
+  }
+
+  /**
+   * Construct a new AprilTagFieldLayout from a list of {@link AprilTag} objects.
+   *
+   * @param apriltags List of {@link AprilTag}.
+   * @param fieldLength Length of the field the layout is representing in meters.
+   * @param fieldWidth Width of the field the layout is representing in meters.
+   */
+  public AprilTagFieldLayout(List<AprilTag> apriltags, double fieldLength, double fieldWidth) {
+    this(apriltags, new FieldDimensions(fieldLength, fieldWidth));
+  }
+
+  @JsonCreator
+  private AprilTagFieldLayout(
+      @JsonProperty(required = true, value = "tags") List<AprilTag> apriltags,
+      @JsonProperty(required = true, value = "field") FieldDimensions fieldDimensions) {
+    // To ensure the underlying semantics don't change with what kind of list is passed in
+    for (AprilTag tag : apriltags) {
+      m_apriltags.put(tag.ID, tag);
+    }
+    m_fieldDimensions = fieldDimensions;
+    setOrigin(OriginPosition.kBlueAllianceWallRightSide);
+  }
+
+  /**
+   * Returns a List of the {@link AprilTag AprilTags} used in this layout.
+   *
+   * @return The {@link AprilTag AprilTags} used in this layout.
+   */
+  @JsonProperty("tags")
+  public List<AprilTag> getTags() {
+    return new ArrayList<>(m_apriltags.values());
+  }
+
+  /**
+   * Sets the origin based on a predefined enumeration of coordinate frame origins. The origins are
+   * calculated from the field dimensions.
+   *
+   * <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
+   */
+  @JsonIgnore
+  public void setOrigin(OriginPosition origin) {
+    switch (origin) {
+      case kBlueAllianceWallRightSide:
+        setOrigin(new Pose3d());
+        break;
+      case kRedAllianceWallRightSide:
+        setOrigin(
+            new Pose3d(
+                new Translation3d(m_fieldDimensions.fieldLength, m_fieldDimensions.fieldWidth, 0),
+                new Rotation3d(0, 0, Math.PI)));
+        break;
+      default:
+        throw new IllegalArgumentException("Unsupported enum value");
+    }
+  }
+
+  /**
+   * Sets the origin for tag pose transformation.
+   *
+   * <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
+   */
+  @JsonIgnore
+  public void setOrigin(Pose3d origin) {
+    m_origin = origin;
+  }
+
+  /**
+   * Gets an AprilTag pose by its ID.
+   *
+   * @param ID The ID of the tag.
+   * @return The pose corresponding to the ID passed in or an empty optional if a tag with that ID
+   *     was not found.
+   */
+  @SuppressWarnings("ParameterName")
+  public Optional<Pose3d> getTagPose(int ID) {
+    AprilTag tag = m_apriltags.get(ID);
+    if (tag == null) {
+      return Optional.empty();
+    }
+    return Optional.of(tag.pose.relativeTo(m_origin));
+  }
+
+  /**
+   * Serializes a AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write to.
+   * @throws IOException If writing to the file fails.
+   */
+  public void serialize(String path) throws IOException {
+    serialize(Path.of(path));
+  }
+
+  /**
+   * Serializes a AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write to.
+   * @throws IOException If writing to the file fails.
+   */
+  public void serialize(Path path) throws IOException {
+    new ObjectMapper().writeValue(path.toFile(), this);
+  }
+
+  /**
+   * 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
+   * @throws IOException If the resource could not be loaded
+   */
+  public static AprilTagFieldLayout loadFromResource(String resourcePath) throws IOException {
+    try (InputStream stream = AprilTagFieldLayout.class.getResourceAsStream(resourcePath);
+        InputStreamReader reader = new InputStreamReader(stream)) {
+      return new ObjectMapper().readerFor(AprilTagFieldLayout.class).readValue(reader);
+    }
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof AprilTagFieldLayout) {
+      var other = (AprilTagFieldLayout) obj;
+      return m_apriltags.equals(other.m_apriltags) && m_origin.equals(other.m_origin);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_apriltags, m_origin);
+  }
+
+  @JsonIgnoreProperties(ignoreUnknown = true)
+  @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+  private static class FieldDimensions {
+    @SuppressWarnings("MemberName")
+    @JsonProperty(value = "length")
+    public double fieldLength;
+
+    @SuppressWarnings("MemberName")
+    @JsonProperty(value = "width")
+    public double fieldWidth;
+
+    @JsonCreator()
+    FieldDimensions(
+        @JsonProperty(required = true, value = "length") double fieldLength,
+        @JsonProperty(required = true, value = "width") double fieldWidth) {
+      this.fieldLength = fieldLength;
+      this.fieldWidth = fieldWidth;
+    }
+  }
+}

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

@@ -0,0 +1,33 @@
+// 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 java.io.IOException;
+
+public enum AprilTagFields {
+  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 = 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

@@ -0,0 +1,90 @@
+// 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.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;
+
+public class AprilTagJNI {
+  static boolean libraryLoaded = false;
+
+  static RuntimeLoader<AprilTagJNI> loader = null;
+
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+  }
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        loader =
+            new RuntimeLoader<>(
+                "apriltagjni", RuntimeLoader.getDefaultExtractionRoot(), AprilTagJNI.class);
+        loader.loadLibrary();
+      } catch (IOException ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  public static native long createDetector();
+
+  public static native void destroyDetector(long det);
+
+  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);
+
+  public static native Transform3d estimatePose(
+      double[] homography,
+      double[] corners,
+      double tagSize,
+      double fx,
+      double fy,
+      double cx,
+      double cy);
+}

apriltag/src/main/native/cpp/AprilTag.cpp

@@ -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.
+
+#include "frc/apriltag/AprilTag.h"
+
+#include <wpi/json.h>
+
+using namespace frc;
+
+void frc::to_json(wpi::json& json, const AprilTag& apriltag) {
+  json = wpi::json{{"ID", apriltag.ID}, {"pose", apriltag.pose}};
+}
+
+void frc::from_json(const wpi::json& json, AprilTag& apriltag) {
+  apriltag.ID = json.at("ID").get<int>();
+  apriltag.pose = json.at("pose").get<Pose3d>();
+}

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

@@ -0,0 +1,107 @@
+// 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/AprilTagFieldLayout.h"
+
+#include <system_error>
+
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
+  std::error_code error_code;
+
+  wpi::raw_fd_istream input{path, error_code};
+  if (error_code) {
+    throw std::runtime_error(fmt::format("Cannot open file: {}", path));
+  }
+
+  wpi::json json;
+  input >> json;
+
+  for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
+    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("length").get<double>()};
+}
+
+AprilTagFieldLayout::AprilTagFieldLayout(std::vector<AprilTag> apriltags,
+                                         units::meter_t fieldLength,
+                                         units::meter_t fieldWidth)
+    : m_fieldLength(std::move(fieldLength)),
+      m_fieldWidth(std::move(fieldWidth)) {
+  for (const auto& tag : apriltags) {
+    m_apriltags[tag.ID] = tag;
+  }
+}
+
+void AprilTagFieldLayout::SetOrigin(OriginPosition origin) {
+  switch (origin) {
+    case OriginPosition::kBlueAllianceWallRightSide:
+      SetOrigin(Pose3d{});
+      break;
+    case OriginPosition::kRedAllianceWallRightSide:
+      SetOrigin(Pose3d{Translation3d{m_fieldLength, m_fieldWidth, 0_m},
+                       Rotation3d{0_deg, 0_deg, 180_deg}});
+      break;
+    default:
+      throw std::invalid_argument("Invalid origin");
+  }
+}
+
+void AprilTagFieldLayout::SetOrigin(const Pose3d& origin) {
+  m_origin = origin;
+}
+
+std::optional<frc::Pose3d> AprilTagFieldLayout::GetTagPose(int ID) const {
+  const auto& it = m_apriltags.find(ID);
+  if (it == m_apriltags.end()) {
+    return std::nullopt;
+  }
+  return it->second.pose.RelativeTo(m_origin);
+}
+
+void AprilTagFieldLayout::Serialize(std::string_view path) {
+  std::error_code error_code;
+
+  wpi::raw_fd_ostream output{path, error_code};
+  if (error_code) {
+    throw std::runtime_error(fmt::format("Cannot open file: {}", path));
+  }
+
+  wpi::json json = *this;
+  output << json;
+  output.flush();
+}
+
+void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) {
+  std::vector<AprilTag> tagVector;
+  tagVector.reserve(layout.m_apriltags.size());
+  for (const auto& pair : layout.m_apriltags) {
+    tagVector.push_back(pair.second);
+  }
+
+  json = wpi::json{{"field",
+                    {{"length", layout.m_fieldLength.value()},
+                     {"width", layout.m_fieldWidth.value()}}},
+                   {"tags", tagVector}};
+}
+
+void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) {
+  layout.m_apriltags.clear();
+  for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
+    layout.m_apriltags[tag.ID] = tag;
+  }
+
+  layout.m_fieldLength =
+      units::meter_t{json.at("field").at("length").get<double>()};
+  layout.m_fieldWidth =
+      units::meter_t{json.at("field").at("width").get<double>()};
+}

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

@@ -0,0 +1,32 @@
+// 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/AprilTagFields.h"
+
+#include <wpi/json.h>
+
+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;
+  switch (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");
+  }
+
+  wpi::json json = wpi::json::parse(fieldString);
+  return json.get<AprilTagFieldLayout>();
+}
+
+}  // namespace frc

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

@@ -0,0 +1,595 @@
+// 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 <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"
+
+using namespace frc;
+using namespace wpi::java;
+
+static JavaVM* jvm = nullptr;
+
+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" {
+
+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;
+  }
+
+  // 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;
+    }
+  }
+
+  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;
+}
+
+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;
+  }
+
+  JLocal<jstring> fam{env, MakeJString(env, detect.GetFamily())};
+
+  auto homography = detect.GetHomography();
+  JLocal<jdoubleArray> harr{
+      env, MakeJDoubleArray(
+               env, {reinterpret_cast<const jdouble*>(homography.data()),
+                     homography.size()})};
+
+  double cornersBuf[8];
+  auto corners = detect.GetCorners(cornersBuf);
+  JLocal<jdoubleArray> carr{
+      env,
+      MakeJDoubleArray(env, {reinterpret_cast<const jdouble*>(corners.data()),
+                             corners.size()})};
+
+  auto center = detect.GetCenter();
+
+  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());
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const AprilTagDetection* const> arr) {
+  jobjectArray jarr = env->NewObjectArray(arr.size(), detectionCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, *arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  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:    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_destroyDetector
+  (JNIEnv* env, jclass, jlong det)
+{
+  delete reinterpret_cast<AprilTagDetector*>(det);
+}
+
+/*
+ * 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;
+  }
+  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/AprilTag.h

@@ -0,0 +1,34 @@
+// 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/Pose3d.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+struct WPILIB_DLLEXPORT AprilTag {
+  int ID;
+
+  Pose3d pose;
+
+  /**
+   * Checks equality between this AprilTag and another object.
+   */
+  bool operator==(const AprilTag&) const = default;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const AprilTag& apriltag);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, AprilTag& apriltag);
+
+}  // namespace frc

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

@@ -0,0 +1,129 @@
+// 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 <optional>
+#include <string_view>
+#include <unordered_map>
+#include <vector>
+
+#include <units/length.h>
+#include <wpi/SymbolExports.h>
+
+#include "frc/apriltag/AprilTag.h"
+#include "frc/geometry/Pose3d.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+/**
+ * Class for representing a layout of AprilTags on a field and reading them from
+ * a JSON format.
+ *
+ * The JSON format contains two top-level objects, "tags" and "field".
+ * The "tags" object is a list of all AprilTags contained within a layout. Each
+ * AprilTag serializes to a JSON object containing an ID and a Pose3d. The
+ * "field" object is a descriptor of the size of the field in meters with
+ * "width" and "length" values.  This is to account for arbitrary field sizes
+ * when transforming the poses.
+ *
+ * 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 {
+    kBlueAllianceWallRightSide,
+    kRedAllianceWallRightSide,
+  };
+
+  AprilTagFieldLayout() = default;
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   */
+  explicit AprilTagFieldLayout(std::string_view path);
+
+  /**
+   * Construct a new AprilTagFieldLayout from a vector of AprilTag objects.
+   *
+   * @param apriltags Vector of AprilTags.
+   * @param fieldLength Length of field the layout is representing.
+   * @param fieldWidth Width of field the layout is representing.
+   */
+  AprilTagFieldLayout(std::vector<AprilTag> apriltags,
+                      units::meter_t fieldLength, units::meter_t fieldWidth);
+
+  /**
+   * Sets the origin based on a predefined enumeration of coordinate frame
+   * origins. The origins are calculated from the field dimensions.
+   *
+   * This transforms the Pose3ds returned by GetTagPose(int) to return the
+   * correct pose relative to a predefined coordinate frame.
+   *
+   * @param origin The predefined origin
+   */
+  void SetOrigin(OriginPosition origin);
+
+  /**
+   * Sets the origin for tag pose transformation.
+   *
+   * This tranforms the Pose3ds returned by GetTagPose(int) to return the
+   * correct pose relative to the provided origin.
+   *
+   * @param origin The new origin for tag transformations
+   */
+  void SetOrigin(const Pose3d& origin);
+
+  /**
+   * Gets an AprilTag pose by its ID.
+   *
+   * @param ID The ID of the tag.
+   * @return The pose corresponding to the ID that was passed in or an empty
+   * optional if a tag with that ID is not found.
+   */
+  std::optional<Pose3d> GetTagPose(int ID) const;
+
+  /**
+   * Serializes an AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write the JSON file to.
+   */
+  void Serialize(std::string_view path);
+
+  /*
+   * Checks equality between this AprilTagFieldLayout and another object.
+   */
+  bool operator==(const AprilTagFieldLayout&) const = default;
+
+ private:
+  std::unordered_map<int, AprilTag> m_apriltags;
+  units::meter_t m_fieldLength;
+  units::meter_t m_fieldWidth;
+  Pose3d m_origin;
+
+  friend WPILIB_DLLEXPORT void to_json(wpi::json& json,
+                                       const AprilTagFieldLayout& layout);
+
+  friend WPILIB_DLLEXPORT void from_json(const wpi::json& json,
+                                         AprilTagFieldLayout& layout);
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const AprilTagFieldLayout& layout);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, AprilTagFieldLayout& layout);
+
+}  // namespace frc

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

@@ -0,0 +1,32 @@
+// 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>
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/apriltag/AprilTagFieldLayout.h"
+
+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
+  kNumFields,
+};
+
+/**
+ * Loads an AprilTagFieldLayout from a predefined field
+ *
+ * @param field The predefined field
+ */
+WPILIB_DLLEXPORT AprilTagFieldLayout
+LoadAprilTagLayoutField(AprilTagField field);
+
+}  // namespace frc

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

@@ -0,0 +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
+          }
+        }
+      }
+    },
+    {
+      "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
+  }
+}

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/AprilTagPoseSetOriginTest.java

@@ -0,0 +1,46 @@
+// 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.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class AprilTagPoseSetOriginTest {
+  @Test
+  void transformationMatches() {
+    var layout =
+        new AprilTagFieldLayout(
+            List.of(
+                new AprilTag(1, new Pose3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0))),
+                new AprilTag(
+                    2,
+                    new Pose3d(
+                        new Translation3d(
+                            Units.feetToMeters(4.0), Units.feetToMeters(4), Units.feetToMeters(4)),
+                        new Rotation3d(0, 0, Units.degreesToRadians(180))))),
+            Units.feetToMeters(54.0),
+            Units.feetToMeters(27.0));
+    layout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide);
+
+    assertEquals(
+        new Pose3d(
+            new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(180.0))),
+        layout.getTagPose(1).orElse(null));
+
+    assertEquals(
+        new Pose3d(
+            new Translation3d(
+                Units.feetToMeters(50.0), Units.feetToMeters(23.0), Units.feetToMeters(4)),
+            new Rotation3d(0.0, 0.0, 0)),
+        layout.getTagPose(2).orElse(null));
+  }
+}

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

@@ -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.
+
+package edu.wpi.first.apriltag;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class AprilTagSerializationTest {
+  @Test
+  void deserializeMatches() {
+    var layout =
+        new AprilTagFieldLayout(
+            List.of(
+                new AprilTag(1, new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0))),
+                new AprilTag(3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0)))),
+            Units.feetToMeters(54.0),
+            Units.feetToMeters(27.0));
+
+    var objectMapper = new ObjectMapper();
+
+    var deserialized =
+        assertDoesNotThrow(
+            () ->
+                objectMapper.readValue(
+                    objectMapper.writeValueAsString(layout), AprilTagFieldLayout.class));
+
+    assertEquals(layout, deserialized);
+  }
+}

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

@@ -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.
+
+package edu.wpi.first.apriltag;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.util.Units;
+import java.io.IOException;
+import java.util.Optional;
+import org.junit.jupiter.api.Assertions;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+class LoadConfigTest {
+  @ParameterizedTest
+  @EnumSource(AprilTagFields.class)
+  void testLoad(AprilTagFields field) {
+    AprilTagFieldLayout layout = Assertions.assertDoesNotThrow(field::loadAprilTagLayoutField);
+    assertNotNull(layout);
+  }
+
+  @Test
+  void test2022RapidReact() throws IOException {
+    AprilTagFieldLayout layout = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField();
+
+    // Blue Hangar Truss - Hub
+    Pose3d expectedPose =
+        new Pose3d(
+            Units.inchesToMeters(127.272),
+            Units.inchesToMeters(216.01),
+            Units.inchesToMeters(67.932),
+            new Rotation3d(0, 0, 0));
+    Optional<Pose3d> maybePose = layout.getTagPose(1);
+    assertTrue(maybePose.isPresent());
+    assertEquals(expectedPose, maybePose.get());
+
+    // Blue Terminal Near Station
+    expectedPose =
+        new Pose3d(
+            Units.inchesToMeters(4.768),
+            Units.inchesToMeters(67.631),
+            Units.inchesToMeters(35.063),
+            new Rotation3d(0, 0, Math.toRadians(46.25)));
+    maybePose = layout.getTagPose(5);
+    assertTrue(maybePose.isPresent());
+    assertEquals(expectedPose, maybePose.get());
+
+    // Upper Hub Blue-Near
+    expectedPose =
+        new Pose3d(
+            Units.inchesToMeters(332.321),
+            Units.inchesToMeters(183.676),
+            Units.inchesToMeters(95.186),
+            new Rotation3d(0, Math.toRadians(26.75), Math.toRadians(69)));
+    maybePose = layout.getTagPose(53);
+    assertTrue(maybePose.isPresent());
+    assertEquals(expectedPose, maybePose.get());
+
+    // Doesn't exist
+    maybePose = layout.getTagPose(54);
+    assertFalse(maybePose.isPresent());
+  }
+}

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");
+}

apriltag/src/test/native/cpp/AprilTagJsonTest.cpp

@@ -0,0 +1,27 @@
+// 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 <vector>
+
+#include <wpi/json.h>
+
+#include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFieldLayout.h"
+#include "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(AprilTagJsonTest, DeserializeMatches) {
+  auto layout = AprilTagFieldLayout{
+      std::vector{
+          AprilTag{1, Pose3d{}},
+          AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}},
+      54_ft, 27_ft};
+
+  AprilTagFieldLayout deserialized;
+  wpi::json json = layout;
+  EXPECT_NO_THROW(deserialized = json.get<AprilTagFieldLayout>());
+  EXPECT_EQ(layout, deserialized);
+}

apriltag/src/test/native/cpp/AprilTagPoseSetOriginTest.cpp

@@ -0,0 +1,33 @@
+// 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 <vector>
+
+#include <wpi/json.h>
+
+#include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFieldLayout.h"
+#include "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(AprilTagPoseSetOriginTest, TransformationMatches) {
+  auto layout = AprilTagFieldLayout{
+      std::vector<AprilTag>{
+          AprilTag{1,
+                   Pose3d{0_ft, 0_ft, 0_ft, Rotation3d{0_deg, 0_deg, 0_deg}}},
+          AprilTag{
+              2, Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}},
+      54_ft, 27_ft};
+
+  layout.SetOrigin(
+      AprilTagFieldLayout::OriginPosition::kRedAllianceWallRightSide);
+
+  auto mirrorPose =
+      Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}};
+  EXPECT_EQ(mirrorPose, *layout.GetTagPose(1));
+  mirrorPose = Pose3d{50_ft, 23_ft, 4_ft, Rotation3d{0_deg, 0_deg, 0_deg}};
+  EXPECT_EQ(mirrorPose, *layout.GetTagPose(2));
+}

apriltag/src/test/native/cpp/LoadConfigTest.cpp

@@ -0,0 +1,61 @@
+// 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/AprilTagFields.h"
+#include "gtest/gtest.h"
+
+namespace frc {
+
+std::vector<AprilTagField> GetAllFields() {
+  std::vector<AprilTagField> output;
+
+  for (int i = 0; i < static_cast<int>(AprilTagField::kNumFields); ++i) {
+    output.push_back(static_cast<AprilTagField>(i));
+  }
+
+  return output;
+}
+
+TEST(AprilTagFieldsTest, TestLoad2022RapidReact) {
+  AprilTagFieldLayout layout =
+      LoadAprilTagLayoutField(AprilTagField::k2022RapidReact);
+
+  // Blue Hangar Truss - Hub
+  auto expectedPose =
+      Pose3d{127.272_in, 216.01_in, 67.932_in, Rotation3d{0_deg, 0_deg, 0_deg}};
+  auto maybePose = layout.GetTagPose(1);
+  EXPECT_TRUE(maybePose);
+  EXPECT_EQ(expectedPose, *maybePose);
+
+  // Blue Terminal Near Station
+  expectedPose = Pose3d{4.768_in, 67.631_in, 35.063_in,
+                        Rotation3d{0_deg, 0_deg, 46.25_deg}};
+  maybePose = layout.GetTagPose(5);
+  EXPECT_TRUE(maybePose);
+  EXPECT_EQ(expectedPose, *maybePose);
+
+  // Upper Hub Blue-Near
+  expectedPose = Pose3d{332.321_in, 183.676_in, 95.186_in,
+                        Rotation3d{0_deg, 26.75_deg, 69_deg}};
+  maybePose = layout.GetTagPose(53);
+  EXPECT_TRUE(maybePose);
+  EXPECT_EQ(expectedPose, *maybePose);
+
+  // Doesn't exist
+  maybePose = layout.GetTagPose(54);
+  EXPECT_FALSE(maybePose);
+}
+
+// Test all of the fields in the enum
+class AllFieldsFixtureTest : public ::testing::TestWithParam<AprilTagField> {};
+
+TEST_P(AllFieldsFixtureTest, CheckEntireEnum) {
+  AprilTagField field = GetParam();
+  EXPECT_NO_THROW(LoadAprilTagLayoutField(field));
+}
+
+INSTANTIATE_TEST_SUITE_P(ValuesEnumTestInstTests, AllFieldsFixtureTest,
+                         ::testing::ValuesIn(GetAllFields()));
+
+}  // namespace frc

apriltag/src/test/native/cpp/main.cpp

@@ -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.
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

azure-pipelines-testbench.yaml

@@ -38,6 +38,7 @@ stages:
 
   - stage: TestBench
     displayName: Test Bench
+    condition: false
     jobs:
       - job: Cpp
         displayName: C++

build.gradle

@@ -13,16 +13,16 @@ buildscript {
 
 plugins {
     id 'base'
-    id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '4.1.0'
+    id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '2023.0.1'
     id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2'
     id 'edu.wpi.first.NativeUtils' apply false
-    id 'edu.wpi.first.GradleJni' version '1.0.0'
+    id 'edu.wpi.first.GradleJni' version '1.1.0'
     id 'edu.wpi.first.GradleVsCode'
     id 'idea'
     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
 }
 
@@ -136,8 +136,10 @@ subprojects {
     }
 
     // Sign outputs with Developer ID
-    if (project.hasProperty("developerID")) {
-        tasks.withType(AbstractLinkTask) { task ->
+    tasks.withType(AbstractLinkTask) { task ->
+        task.inputs.property "HasDeveloperId", project.hasProperty("developerID")
+
+        if (project.hasProperty("developerID")) {
             // Don't sign any executables because codesign complains
             // about relative rpath.
             if (!(task instanceof LinkExecutable)) {

buildSrc/build.gradle

@@ -1,9 +1,13 @@
 repositories {
     maven {
-        url "https://plugins.gradle.org/m2/"
         mavenLocal()
+        maven {
+            url = 'https://frcmaven.wpi.edu/artifactory/ex-gradle'
+        }
+        mavenCentral()
+        url "https://plugins.gradle.org/m2/"
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2023.4.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"
 
@@ -61,8 +61,13 @@ model {
             if (!it.buildable || !(it instanceof NativeBinarySpec)) {
                 return
             }
-            lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
-            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            if (it.component.name == "${nativeName}JNI") {
+                lib project: ':wpinet', library: 'wpinet', linkage: 'static'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+            } else {
+                lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            }
         }
     }
 }
@@ -82,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'
@@ -152,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 = [
@@ -176,6 +187,8 @@ nativeUtils.exportsConfigs {
     }
 }
 
+apply from: "${rootDir}/shared/imgui.gradle"
+
 model {
     components {
         examplesMap.each { key, value ->
@@ -188,7 +201,7 @@ model {
                             lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
                             lib project: ':wpigui', library: 'wpigui', linkage: 'static'
                             lib library: 'cscore', linkage: 'shared'
-                            nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                            nativeUtils.useRequiredLibrary(it, 'imgui')
                             if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
                                 it.buildable = false
                                 return

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/java/edu/wpi/first/cscore/VideoMode.java

@@ -13,7 +13,9 @@ public class VideoMode {
     kYUYV(2),
     kRGB565(3),
     kBGR(4),
-    kGray(5);
+    kGray(5),
+    kY16(6),
+    kUYVY(7);
 
     private final int value;
 

cscore/src/main/native/cpp/Frame.cpp

@@ -217,7 +217,7 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
   // Color convert
   switch (pixelFormat) {
     case VideoMode::kRGB565:
-      // If source is YUYV or Gray, need to convert to BGR first
+      // If source is YUYV, UYVY, Gray, or Y16, need to convert to BGR first
       if (cur->pixelFormat == VideoMode::kYUYV) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
@@ -226,6 +226,14 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
         } else {
           cur = ConvertYUYVToBGR(cur);
         }
+      } else if (cur->pixelFormat == VideoMode::kUYVY) {
+        // Check to see if BGR version already exists...
+        if (Image* newImage =
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
+          cur = newImage;
+        } else {
+          cur = ConvertUYVYToBGR(cur);
+        }
       } else if (cur->pixelFormat == VideoMode::kGray) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
@@ -234,18 +242,35 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
         } else {
           cur = ConvertGrayToBGR(cur);
         }
-      }
-      return ConvertBGRToRGB565(cur);
-    case VideoMode::kGray:
-      // If source is YUYV or RGB565, need to convert to BGR first
-      if (cur->pixelFormat == VideoMode::kYUYV) {
+      } else if (cur->pixelFormat == VideoMode::kY16) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
                 GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
           cur = newImage;
+        } else if (Image* newImage = GetExistingImage(cur->width, cur->height,
+                                                      VideoMode::kGray)) {
+          cur = ConvertGrayToBGR(newImage);
         } else {
-          cur = ConvertYUYVToBGR(cur);
+          cur = ConvertGrayToBGR(ConvertY16ToGray(cur));
         }
+      }
+      return ConvertBGRToRGB565(cur);
+    case VideoMode::kGray:
+    case VideoMode::kY16:
+      // If source is also grayscale, convert directly
+      if (pixelFormat == VideoMode::kGray &&
+          cur->pixelFormat == VideoMode::kY16) {
+        return ConvertY16ToGray(cur);
+      } else if (pixelFormat == VideoMode::kY16 &&
+                 cur->pixelFormat == VideoMode::kGray) {
+        return ConvertGrayToY16(cur);
+      }
+      // If source is YUYV, UYVY, convert directly to Gray
+      // If RGB565, need to convert to BGR first
+      if (cur->pixelFormat == VideoMode::kYUYV) {
+        cur = ConvertYUYVToGray(cur);
+      } else if (cur->pixelFormat == VideoMode::kUYVY) {
+        cur = ConvertUYVYToGray(cur);
       } else if (cur->pixelFormat == VideoMode::kRGB565) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
@@ -254,12 +279,18 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
         } else {
           cur = ConvertRGB565ToBGR(cur);
         }
+        cur = ConvertBGRToGray(cur);
       }
-      return ConvertBGRToGray(cur);
+      if (pixelFormat == VideoMode::kY16) {
+        cur = ConvertGrayToY16(cur);
+      }
+      return cur;
     case VideoMode::kBGR:
     case VideoMode::kMJPEG:
       if (cur->pixelFormat == VideoMode::kYUYV) {
         cur = ConvertYUYVToBGR(cur);
+      } else if (cur->pixelFormat == VideoMode::kUYVY) {
+        cur = ConvertUYVYToBGR(cur);
       } else if (cur->pixelFormat == VideoMode::kRGB565) {
         cur = ConvertRGB565ToBGR(cur);
       } else if (cur->pixelFormat == VideoMode::kGray) {
@@ -268,9 +299,23 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
         } else {
           return ConvertGrayToMJPEG(cur, defaultJpegQuality);
         }
+      } else if (cur->pixelFormat == VideoMode::kY16) {
+        // Check to see if Gray version already exists...
+        if (Image* newImage =
+                GetExistingImage(cur->width, cur->height, VideoMode::kGray)) {
+          cur = newImage;
+        } else {
+          cur = ConvertY16ToGray(cur);
+        }
+        if (pixelFormat == VideoMode::kBGR) {
+          return ConvertGrayToBGR(cur);
+        } else {
+          return ConvertGrayToMJPEG(cur, defaultJpegQuality);
+        }
       }
       break;
     case VideoMode::kYUYV:
+    case VideoMode::kUYVY:
     default:
       return nullptr;  // Unsupported
   }
@@ -351,6 +396,72 @@ Image* Frame::ConvertYUYVToBGR(Image* image) {
   return rv;
 }
 
+Image* Frame::ConvertYUYVToGray(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kYUYV) {
+    return nullptr;
+  }
+
+  // Allocate a grayscale image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kGray, image->width, image->height,
+                                image->width * image->height);
+
+  // Convert
+  cv::cvtColor(image->AsMat(), newImage->AsMat(), cv::COLOR_YUV2GRAY_YUYV);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
+Image* Frame::ConvertUYVYToBGR(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kUYVY) {
+    return nullptr;
+  }
+
+  // Allocate a BGR image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kBGR, image->width, image->height,
+                                image->width * image->height * 3);
+
+  // Convert
+  cv::cvtColor(image->AsMat(), newImage->AsMat(), cv::COLOR_YUV2BGR_UYVY);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
+Image* Frame::ConvertUYVYToGray(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kUYVY) {
+    return nullptr;
+  }
+
+  // Allocate a grayscale image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kGray, image->width, image->height,
+                                image->width * image->height);
+
+  // Convert
+  cv::cvtColor(image->AsMat(), newImage->AsMat(), cv::COLOR_YUV2GRAY_UYVY);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
 Image* Frame::ConvertBGRToRGB565(Image* image) {
   if (!image || image->pixelFormat != VideoMode::kBGR) {
     return nullptr;
@@ -509,6 +620,50 @@ Image* Frame::ConvertGrayToMJPEG(Image* image, int quality) {
   return rv;
 }
 
+Image* Frame::ConvertGrayToY16(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kGray) {
+    return nullptr;
+  }
+
+  // Allocate a Y16 image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kY16, image->width, image->height,
+                                image->width * image->height * 2);
+
+  // Convert with linear scaling
+  image->AsMat().convertTo(newImage->AsMat(), CV_16U, 256);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
+Image* Frame::ConvertY16ToGray(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kY16) {
+    return nullptr;
+  }
+
+  // Allocate a Grayscale image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kGray, image->width, image->height,
+                                image->width * image->height);
+
+  // Scale min to 0 and max to 255
+  cv::normalize(image->AsMat(), newImage->AsMat(), 255, 0, cv::NORM_MINMAX);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
 Image* Frame::GetImageImpl(int width, int height,
                            VideoMode::PixelFormat pixelFormat,
                            int requiredJpegQuality, int defaultJpegQuality) {

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

@@ -195,12 +195,17 @@ class Frame {
   Image* ConvertMJPEGToBGR(Image* image);
   Image* ConvertMJPEGToGray(Image* image);
   Image* ConvertYUYVToBGR(Image* image);
+  Image* ConvertYUYVToGray(Image* image);
+  Image* ConvertUYVYToBGR(Image* image);
+  Image* ConvertUYVYToGray(Image* image);
   Image* ConvertBGRToRGB565(Image* image);
   Image* ConvertRGB565ToBGR(Image* image);
   Image* ConvertBGRToGray(Image* image);
   Image* ConvertGrayToBGR(Image* image);
   Image* ConvertBGRToMJPEG(Image* image, int quality);
   Image* ConvertGrayToMJPEG(Image* image, int quality);
+  Image* ConvertGrayToY16(Image* image);
+  Image* ConvertY16ToGray(Image* image);
 
   Image* GetImage(int width, int height, VideoMode::PixelFormat pixelFormat) {
     if (pixelFormat == VideoMode::kMJPEG) {

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

@@ -74,6 +74,8 @@ class Image {
     switch (pixelFormat) {
       case VideoMode::kYUYV:
       case VideoMode::kRGB565:
+      case VideoMode::kY16:
+      case VideoMode::kUYVY:
         type = CV_8UC2;
         break;
       case VideoMode::kBGR:

cscore/src/main/native/cpp/MjpegServerImpl.cpp

@@ -460,6 +460,12 @@ void MjpegServerImpl::ConnThread::SendHTML(wpi::raw_ostream& os,
       case VideoMode::kGray:
         os << "gray";
         break;
+      case VideoMode::kY16:
+        os << "Y16";
+        break;
+      case VideoMode::kUYVY:
+        os << "UYVY";
+        break;
       default:
         os << "unknown";
         break;
@@ -569,6 +575,12 @@ void MjpegServerImpl::ConnThread::SendJSON(wpi::raw_ostream& os,
       case VideoMode::kGray:
         os << "gray";
         break;
+      case VideoMode::kY16:
+        os << "Y16";
+        break;
+      case VideoMode::kUYVY:
+        os << "UYVY";
+        break;
       default:
         os << "unknown";
         break;
@@ -740,8 +752,10 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
         // for adding it if required.
         addDHT = JpegNeedsDHT(data, &size, &locSOF);
         break;
-      case VideoMode::kYUYV:
+      case VideoMode::kUYVY:
       case VideoMode::kRGB565:
+      case VideoMode::kYUYV:
+      case VideoMode::kY16:
       default:
         // Bad frame; sleep for 10 ms so we don't consume all processor time.
         std::this_thread::sleep_for(std::chrono::milliseconds(10));

cscore/src/main/native/cpp/RawSourceImpl.cpp

@@ -26,6 +26,8 @@ void RawSourceImpl::PutFrame(const CS_RawFrame& image) {
   switch (image.pixelFormat) {
     case VideoMode::kYUYV:
     case VideoMode::kRGB565:
+    case VideoMode::kY16:
+    case VideoMode::kUYVY:
       type = CV_8UC2;
       break;
     case VideoMode::kBGR:

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

@@ -198,6 +198,10 @@ bool SourceImpl::SetConfigJson(const wpi::json& config, CS_Status* status) {
         mode.pixelFormat = cs::VideoMode::kBGR;
       } else if (wpi::equals_lower(str, "gray")) {
         mode.pixelFormat = cs::VideoMode::kGray;
+      } else if (wpi::equals_lower(str, "y16")) {
+        mode.pixelFormat = cs::VideoMode::kY16;
+      } else if (wpi::equals_lower(str, "uyvy")) {
+        mode.pixelFormat = cs::VideoMode::kUYVY;
       } else {
         SWARNING("SetConfigJson: could not understand pixel format value '{}'",
                  str);
@@ -360,6 +364,12 @@ wpi::json SourceImpl::GetConfigJsonObject(CS_Status* status) {
     case VideoMode::kGray:
       pixelFormat = "gray";
       break;
+    case VideoMode::kY16:
+      pixelFormat = "y16";
+      break;
+    case VideoMode::kUYVY:
+      pixelFormat = "uyvy";
+      break;
     default:
       break;
   }

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_c.h

@@ -93,7 +93,9 @@ enum CS_PixelFormat {
   CS_PIXFMT_YUYV,
   CS_PIXFMT_RGB565,
   CS_PIXFMT_BGR,
-  CS_PIXFMT_GRAY
+  CS_PIXFMT_GRAY,
+  CS_PIXFMT_Y16,
+  CS_PIXFMT_UYVY
 };
 
 /**

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

@@ -68,7 +68,9 @@ struct VideoMode : public CS_VideoMode {
     kYUYV = CS_PIXFMT_YUYV,
     kRGB565 = CS_PIXFMT_RGB565,
     kBGR = CS_PIXFMT_BGR,
-    kGray = CS_PIXFMT_GRAY
+    kGray = CS_PIXFMT_GRAY,
+    kY16 = CS_PIXFMT_Y16,
+    kUYVY = CS_PIXFMT_UYVY
   };
   VideoMode() {
     pixelFormat = 0;
@@ -89,7 +91,10 @@ struct VideoMode : public CS_VideoMode {
            height == other.height && fps == other.fps;
   }
 
-  bool operator!=(const VideoMode& other) const { return !(*this == other); }
+  bool CompareWithoutFps(const VideoMode& other) const {
+    return pixelFormat == other.pixelFormat && width == other.width &&
+           height == other.height;
+  }
 };
 
 /**

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

@@ -140,8 +140,6 @@ class VideoSource {
     return m_handle == other.m_handle;
   }
 
-  bool operator!=(const VideoSource& other) const { return !(*this == other); }
-
   /**
    * Get the kind of the source.
    */
@@ -736,8 +734,6 @@ class VideoSink {
     return m_handle == other.m_handle;
   }
 
-  bool operator!=(const VideoSink& other) const { return !(*this == other); }
-
   /**
    * Get the kind of the sink.
    */

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

@@ -4,10 +4,8 @@
 
 #pragma once
 
-class Switch {
- public:
-  virtual ~Switch() = default;
-
-  /// \brief Returns true when the switch is triggered.
-  virtual bool Get() = 0;
-};
+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/linux/UsbCameraImpl.cpp

@@ -82,6 +82,10 @@ static VideoMode::PixelFormat ToPixelFormat(__u32 pixelFormat) {
       return VideoMode::kBGR;
     case V4L2_PIX_FMT_GREY:
       return VideoMode::kGray;
+    case V4L2_PIX_FMT_Y16:
+      return VideoMode::kY16;
+    case V4L2_PIX_FMT_UYVY:
+      return VideoMode::kUYVY;
     default:
       return VideoMode::kUnknown;
   }
@@ -100,6 +104,10 @@ static __u32 FromPixelFormat(VideoMode::PixelFormat pixelFormat) {
       return V4L2_PIX_FMT_BGR24;
     case VideoMode::kGray:
       return V4L2_PIX_FMT_GREY;
+    case VideoMode::kY16:
+      return V4L2_PIX_FMT_Y16;
+    case VideoMode::kUYVY:
+      return V4L2_PIX_FMT_UYVY;
     default:
       return 0;
   }
@@ -140,6 +148,12 @@ int UsbCameraImpl::RawToPercentage(const UsbCameraProperty& rawProp,
     }
     return 100;
   }
+  // Arducam OV9281 exposure setting quirk
+  if (m_ov9281_exposure && rawProp.name == "raw_exposure_absolute" &&
+      rawProp.minimum == 1 && rawProp.maximum == 5000) {
+    // real range is 1-75
+    return 100.0 * (rawValue - 1) / (75 - 1);
+  }
   return 100.0 * (rawValue - rawProp.minimum) /
          (rawProp.maximum - rawProp.minimum);
 }
@@ -159,6 +173,12 @@ int UsbCameraImpl::PercentageToRaw(const UsbCameraProperty& rawProp,
     }
     return quirkLifeCamHd3000[ndx];
   }
+  // Arducam OV9281 exposure setting quirk
+  if (m_ov9281_exposure && rawProp.name == "raw_exposure_absolute" &&
+      rawProp.minimum == 1 && rawProp.maximum == 5000) {
+    // real range is 1-75
+    return 1 + (75 - 1) * (percentValue / 100.0);
+  }
   return rawProp.minimum +
          (rawProp.maximum - rawProp.minimum) * (percentValue / 100.0);
 }
@@ -1384,6 +1404,7 @@ void UsbCameraImpl::SetQuirks() {
   std::string_view desc = GetDescription(descbuf);
   m_lifecam_exposure = wpi::ends_with(desc, "LifeCam HD-3000") ||
                        wpi::ends_with(desc, "LifeCam Cinema (TM)");
+  m_ov9281_exposure = wpi::contains(desc, "OV9281");
   m_picamera = wpi::ends_with(desc, "mmal service");
 
   int deviceNum = GetDeviceNum(m_path.c_str());

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

@@ -161,6 +161,7 @@ class UsbCameraImpl : public SourceImpl {
   // Quirks
   bool m_lifecam_exposure{false};    // Microsoft LifeCam exposure
   bool m_ps3eyecam_exposure{false};  // PS3 Eyecam exposure
+  bool m_ov9281_exposure{false};     // Arducam OV9281 exposure
   bool m_picamera{false};            // Raspberry Pi camera
 
   //

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,41 +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 "cscore_cpp.h"
-
-namespace cs {
-
-CS_Source CreateUsbCameraDev(std::string_view name, int dev,
-                             CS_Status* status) {
-  *status = CS_INVALID_HANDLE;
-  return 0;
-}
-
-CS_Source CreateUsbCameraPath(std::string_view name, std::string_view path,
-                              CS_Status* status) {
-  *status = CS_INVALID_HANDLE;
-  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;
-  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

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

@@ -356,6 +356,12 @@ void UsbCameraImpl::ProcessFrame(IMFSample* videoSample,
                         tmpMat.total());
       tmpMat.copyTo(dest->AsMat());
       break;
+    case cs::VideoMode::PixelFormat::kY16:
+      tmpMat = cv::Mat(mode.height, mode.width, CV_8UC2, ptr, pitch);
+      dest =
+          AllocImage(VideoMode::kY16, tmpMat.cols, tmpMat.rows, tmpMat.total());
+      tmpMat.copyTo(dest->AsMat());
+      break;
     case cs::VideoMode::PixelFormat::kBGR:
       tmpMat = cv::Mat(mode.height, mode.width, CV_8UC3, ptr, pitch);
       dest = AllocImage(VideoMode::kBGR, tmpMat.cols, tmpMat.rows,
@@ -368,6 +374,12 @@ void UsbCameraImpl::ProcessFrame(IMFSample* videoSample,
                         tmpMat.total() * 2);
       tmpMat.copyTo(dest->AsMat());
       break;
+    case cs::VideoMode::PixelFormat::kUYVY:
+      tmpMat = cv::Mat(mode.height, mode.width, CV_8UC2, ptr, pitch);
+      dest = AllocImage(VideoMode::kUYVY, tmpMat.cols, tmpMat.rows,
+                        tmpMat.total() * 2);
+      tmpMat.copyTo(dest->AsMat());
+      break;
     default:
       doFinalSet = false;
       break;
@@ -461,9 +473,10 @@ LRESULT UsbCameraImpl::PumpMain(HWND hwnd, UINT uiMsg, WPARAM wParam,
 
 static cs::VideoMode::PixelFormat GetFromGUID(const GUID& guid) {
   // Compare GUID to one of the supported ones
-  if (IsEqualGUID(guid, MFVideoFormat_NV12)) {
-    // GrayScale
+  if (IsEqualGUID(guid, MFVideoFormat_L8)) {
     return cs::VideoMode::PixelFormat::kGray;
+  } else if (IsEqualGUID(guid, MFVideoFormat_L16)) {
+    return cs::VideoMode::PixelFormat::kY16;
   } else if (IsEqualGUID(guid, MFVideoFormat_YUY2)) {
     return cs::VideoMode::PixelFormat::kYUYV;
   } else if (IsEqualGUID(guid, MFVideoFormat_RGB24)) {
@@ -472,6 +485,8 @@ static cs::VideoMode::PixelFormat GetFromGUID(const GUID& guid) {
     return cs::VideoMode::PixelFormat::kMJPEG;
   } else if (IsEqualGUID(guid, MFVideoFormat_RGB565)) {
     return cs::VideoMode::PixelFormat::kRGB565;
+  } else if (IsEqualGUID(guid, MFVideoFormat_UYVY)) {
+    return cs::VideoMode::PixelFormat::kUYVY;
   } else {
     return cs::VideoMode::PixelFormat::kUnknown;
   }

datalogtool/build.gradle

@@ -25,6 +25,7 @@ if (!project.hasProperty('onlylinuxathena')) {
     def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp")
 
     apply from: "${rootDir}/shared/libssh.gradle"
+    apply from: "${rootDir}/shared/imgui.gradle"
 
     task generateCppVersion() {
         description = 'Generates the wpilib version class'
@@ -103,7 +104,7 @@ if (!project.hasProperty('onlylinuxathena')) {
                     lib project: ':glass', library: 'glass', linkage: 'static'
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
                     lib project: ':wpigui', library: 'wpigui', linkage: 'static'
-                    nativeUtils.useRequiredLibrary(it, 'imgui_static', 'libssh')
+                    nativeUtils.useRequiredLibrary(it, 'imgui', 'libssh')
                     if (it.targetPlatform.operatingSystem.isWindows()) {
                         it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
                         it.linker.args << 'ws2_32.lib' << 'advapi32.lib' << 'crypt32.lib' << 'user32.lib'

datalogtool/publish.gradle

@@ -37,6 +37,8 @@ model {
                             into("MacOS") { with copySpec { from binary.executable.file } }
                             into("Resources") { with copySpec { from icon } }
 
+                            inputs.property "HasDeveloperId", project.hasProperty("developerID")
+
                             doLast {
                                 if (project.hasProperty("developerID")) {
                                     // Get path to binary.