Initial AprilTag support (#458)

(Very) beta AprilTag support in PhotonVision. Disables Picam GPU acceleration until we can debug auto exposure in the MMAL driver.

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>
Co-authored-by: Chris <chrisgerth010592@gmail.com>
Co-authored-by: mdurrani808 <mdurrani808@gmail.com>

photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java

@@ -20,7 +20,6 @@ package org.photonvision.common.configuration;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.fasterxml.jackson.annotation.JsonProperty;
-import edu.wpi.first.math.geometry.Rotation2d;
 import java.util.ArrayList;
 import java.util.List;
 import org.photonvision.common.logging.LogGroup;
@@ -50,7 +49,6 @@ public class CameraConfiguration {
     public double FOV = 70;
     public final List<CameraCalibrationCoefficients> calibrations;
     public int currentPipelineIndex = 0;
-    public Rotation2d camPitch = new Rotation2d();
 
     public int streamIndex = 0; // 0 index means ports [1181, 1182], 1 means [1183, 1184], etc...
 
@@ -90,8 +88,7 @@ public class CameraConfiguration {
             @JsonProperty("path") String path,
             @JsonProperty("cameraType") CameraType cameraType,
             @JsonProperty("calibration") List<CameraCalibrationCoefficients> calibrations,
-            @JsonProperty("currentPipelineIndex") int currentPipelineIndex,
-            @JsonProperty("camPitch") Rotation2d camPitch) {
+            @JsonProperty("currentPipelineIndex") int currentPipelineIndex) {
         this.baseName = baseName;
         this.uniqueName = uniqueName;
         this.nickname = nickname;
@@ -100,7 +97,6 @@ public class CameraConfiguration {
         this.cameraType = cameraType;
         this.calibrations = calibrations != null ? calibrations : new ArrayList<>();
         this.currentPipelineIndex = currentPipelineIndex;
-        this.camPitch = camPitch;
 
         logger.debug(
                 "Creating camera configuration for "

photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java

@@ -128,7 +128,8 @@ public class PhotonConfiguration {
 
     public static class UICameraConfiguration {
         @SuppressWarnings("unused")
-        public double fov, tiltDegrees;
+        public double fov;
+
         public String nickname;
         public HashMap<String, Object> currentPipelineSettings;
         public int currentPipelineIndex;

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

@@ -189,10 +189,17 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
             targetAreaEntry.forceSetDouble(bestTarget.getArea());
             targetSkewEntry.forceSetDouble(bestTarget.getSkew());
 
-            var poseX = bestTarget.getCameraToTarget().getTranslation().getX();
-            var poseY = bestTarget.getCameraToTarget().getTranslation().getY();
-            var poseRot = bestTarget.getCameraToTarget().getRotation().getDegrees();
-            targetPoseEntry.forceSetDoubleArray(new double[] {poseX, poseY, poseRot});
+            var pose = bestTarget.getCameraToTarget3d();
+            targetPoseEntry.forceSetDoubleArray(
+                    new double[] {
+                        pose.getTranslation().getX(),
+                        pose.getTranslation().getY(),
+                        pose.getTranslation().getZ(),
+                        pose.getRotation().getQuaternion().getW(),
+                        pose.getRotation().getQuaternion().getX(),
+                        pose.getRotation().getQuaternion().getY(),
+                        pose.getRotation().getQuaternion().getZ()
+                    });
 
             var targetOffsetPoint = bestTarget.getTargetOffsetPoint();
             bestTargetPosX.forceSetDouble(targetOffsetPoint.x);
@@ -224,7 +231,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
                             t.getPitch(),
                             t.getArea(),
                             t.getSkew(),
-                            t.getCameraToTarget(),
+                            t.getFiducialId(),
+                            t.getCameraToTarget3d(),
                             cornerList));
         }
         return ret;

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

@@ -49,20 +49,20 @@ public enum Platform {
 
     // These are queried on init and should never change after
     public static final Platform currentPlatform = getCurrentPlatform();
-    protected static final String currentPiVersionStr = getPiVersionString();
+    static final String currentPiVersionStr = getPiVersionString();
     public static final PiVersion currentPiVersion = PiVersion.getPiVersion();
 
-    private static String UnknownPlatformString =
+    private static final String UnknownPlatformString =
             String.format("Unknown Platform. OS: %s, Architecture: %s", OS_NAME, OS_ARCH);
 
-    public boolean isWindows() {
-        return this == WINDOWS_64 || this == WINDOWS_32;
+    public static boolean isWindows() {
+        return currentPlatform == WINDOWS_64 || currentPlatform == WINDOWS_32;
     }
 
     public static boolean isLinux() {
-        return getCurrentPlatform() == LINUX_64
-                || getCurrentPlatform() == LINUX_RASPBIAN
-                || getCurrentPlatform() == LINUX_ARM64;
+        return currentPlatform == LINUX_64
+                || currentPlatform == LINUX_RASPBIAN
+                || currentPlatform == LINUX_ARM64;
     }
 
     public static boolean isRaspberryPi() {

photon-core/src/main/java/org/photonvision/common/networking/NetworkManager.java

@@ -46,7 +46,7 @@ public class NetworkManager {
 
         var config = ConfigManager.getInstance().getConfig().getNetworkConfig();
         logger.info("Setting " + config.connectionType + " with team team " + config.teamNumber);
-        if (Platform.isLinux()) {
+        if (Platform.isRaspberryPi()) {
             if (!Platform.isRoot) {
                 logger.error("Cannot manage network without root!");
                 return;

photon-core/src/main/java/org/photonvision/common/util/NativeLibHelper.java

@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.util;
+
+import java.nio.file.Path;
+import java.nio.file.Paths;
+
+public class NativeLibHelper {
+    private static NativeLibHelper INSTANCE;
+
+    public static NativeLibHelper getInstance() {
+        if (INSTANCE == null) {
+            INSTANCE = new NativeLibHelper();
+        }
+
+        return INSTANCE;
+    }
+
+    public final Path NativeLibPath;
+
+    private NativeLibHelper() {
+        String home = System.getProperty("user.home");
+        NativeLibPath = Paths.get(home, ".pvlibs", "nativecache");
+    }
+}

photon-core/src/main/java/org/photonvision/common/util/TestUtils.java

@@ -29,6 +29,16 @@ import org.opencv.highgui.HighGui;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 
 public class TestUtils {
+    public static void loadLibraries() {
+        try {
+            CameraServerCvJNI.forceLoad();
+            //        PicamJNI.forceLoad();
+            // AprilTagJNI.forceLoad();
+        } catch (IOException ex) {
+            // ignored
+        }
+    }
+
     @SuppressWarnings("unused")
     public enum WPI2019Image {
         kCargoAngledDark48in(1.2192),
@@ -154,6 +164,22 @@ public class TestUtils {
         }
     }
 
+    public enum ApriltagTestImages {
+        kRobots;
+
+        public final Path path;
+
+        Path getPath() {
+            // Strip leading k
+            var filename = this.toString().substring(1).toLowerCase();
+            return Path.of("apriltag", filename + ".jpg");
+        }
+
+        ApriltagTestImages() {
+            this.path = getPath();
+        }
+    }
+
     private static Path getResourcesFolderPath(boolean testMode) {
         System.out.println("CWD: " + Path.of("").toAbsolutePath().toString());
         return Path.of("test-resources").toAbsolutePath();
@@ -177,6 +203,12 @@ public class TestUtils {
                 .resolve(WPI2022Image.kTerminal22ft6in.path);
     }
 
+    public static Path getTestModeApriltagPath() {
+        return getResourcesFolderPath(true)
+                .resolve("testimages")
+                .resolve(ApriltagTestImages.kRobots.path);
+    }
+
     public static Path getTestImagesPath(boolean testMode) {
         return getResourcesFolderPath(testMode).resolve("testimages");
     }
@@ -243,12 +275,8 @@ public class TestUtils {
         return getCoeffs(LIFECAM_480P_CAL_FILE, testMode);
     }
 
-    public static void loadLibraries() {
-        try {
-            CameraServerCvJNI.forceLoad();
-        } catch (IOException e) {
-            // ignored
-        }
+    public static CameraCalibrationCoefficients getLaptop() {
+        return getCoeffs("laptop.json", true);
     }
 
     private static int DefaultTimeoutMillis = 5000;

photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java

@@ -17,6 +17,13 @@
 
 package org.photonvision.common.util.math;
 
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.geometry.CoordinateSystem;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.Arrays;
 import java.util.List;
@@ -130,4 +137,47 @@ public class MathUtils {
     public static double lerp(double startValue, double endValue, double t) {
         return startValue + (endValue - startValue) * t;
     }
+
+    public static Pose3d EDNtoNWU(final Pose3d pose) {
+        // Change of basis matrix from EDN to NWU. Each column vector is one of the
+        // old basis vectors mapped to its representation in the new basis.
+        //
+        // E (+X) -> N (-Y), D (+Y) -> W (-Z), N (+Z) -> U (+X)
+        var R = new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0);
+
+        // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+        double w = Math.sqrt(1.0 + R.get(0, 0) + R.get(1, 1) + R.get(2, 2)) / 2.0;
+        double x = (R.get(2, 1) - R.get(1, 2)) / (4.0 * w);
+        double y = (R.get(0, 2) - R.get(2, 0)) / (4.0 * w);
+        double z = (R.get(1, 0) - R.get(0, 1)) / (4.0 * w);
+        var rotationQuat = new Rotation3d(new Quaternion(w, x, y, z));
+
+        return new Pose3d(
+                pose.getTranslation().rotateBy(rotationQuat), pose.getRotation().rotateBy(rotationQuat));
+    }
+
+    // TODO: Refactor into new pipe?
+    public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
+        // CameraToTarget _should_ be in opencv-land EDN
+
+        var pose =
+                CoordinateSystem.convert(
+                        new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
+
+        return pose;
+    }
+
+    public static Pose3d convertApriltagtoPhotonPose(Transform3d cameraToTarget3d) {
+        // CameraToTarget _should_ be in opencv-land EDN
+        var pose =
+                CoordinateSystem.convert(
+                        new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
+
+        // Apply an extra rotation so that at zero pose, X ls left, Y is up, and Z is towards the camera
+        // to a camera facing along the +X axis of the field parallel with the ground plane
+        // So we need a 180 flip about X axis
+        var newRotation = pose.getRotation().rotateBy(new Rotation3d(0, Math.PI, 0));
+
+        return new Pose3d(pose.getTranslation(), newRotation);
+    }
 }

photon-core/src/main/java/org/photonvision/raspi/PicamJNI.java

@@ -30,6 +30,8 @@ import org.photonvision.common.logging.Logger;
 
 public class PicamJNI {
     private static boolean libraryLoaded = false;
+    private static boolean enabled =
+            false; // TODO once we've sorted out what apriltags needs to be doing, we can bring this back?
     private static Logger logger = new Logger(PicamJNI.class, LogGroup.Camera);
 
     public enum SensorModel {
@@ -86,6 +88,7 @@ public class PicamJNI {
 
     public static boolean isSupported() {
         return libraryLoaded
+                && enabled
                 && isVCSMSupported()
                 && getSensorModel() != SensorModel.Disconnected
                 && Platform.isRaspberryPi()

photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetector.java

@@ -0,0 +1,92 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.apriltag;
+
+import org.opencv.core.Mat;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+
+public class AprilTagDetector {
+    private static final Logger logger = new Logger(AprilTagDetector.class, LogGroup.VisionModule);
+    private long m_detectorPtr = 0;
+    private AprilTagDetectorParams m_detectorParams = AprilTagDetectorParams.DEFAULT_36H11;
+
+    public AprilTagDetector() {
+        updateDetector();
+    }
+
+    private void updateDetector() {
+        if (m_detectorPtr != 0) {
+            // TODO: in JNI
+            AprilTagJNI.AprilTag_Destroy(m_detectorPtr);
+            m_detectorPtr = 0;
+        }
+
+        logger.debug("Creating detector with params " + m_detectorParams);
+        m_detectorPtr =
+                AprilTagJNI.AprilTag_Create(
+                        m_detectorParams.tagFamily.getNativeName(),
+                        m_detectorParams.decimate,
+                        m_detectorParams.blur,
+                        m_detectorParams.threads,
+                        m_detectorParams.debug,
+                        m_detectorParams.refineEdges);
+    }
+
+    public void updateParams(AprilTagDetectorParams newParams) {
+        if (!m_detectorParams.equals(newParams)) {
+            m_detectorParams = newParams;
+            updateDetector();
+        }
+    }
+
+    public DetectionResult[] detect(
+            Mat grayscaleImg,
+            CameraCalibrationCoefficients coeffs,
+            boolean useNativePoseEst,
+            int numIterations,
+            double tagWidthMeters) {
+        if (m_detectorPtr == 0) {
+            // Detector not set up (JNI issue? or similar?)
+            // No detection is possible.
+            return new DetectionResult[] {};
+        }
+
+        var cx = 0.0;
+        var cy = 0.0;
+        var fx = 0.0;
+        var fy = 0.0;
+        var doPoseEst = false;
+
+        if (coeffs != null && useNativePoseEst) {
+            final Mat cameraMatrix = coeffs.getCameraIntrinsicsMat();
+            if (cameraMatrix != null) {
+                // Camera calibration has been done, we should be able to do pose estimation
+                cx = cameraMatrix.get(0, 2)[0];
+                cy = cameraMatrix.get(1, 2)[0];
+                fx = cameraMatrix.get(0, 0)[0];
+                fy = cameraMatrix.get(1, 1)[0];
+                doPoseEst = true;
+            }
+        }
+
+        return AprilTagJNI.AprilTag_Detect(
+                m_detectorPtr, grayscaleImg, doPoseEst, tagWidthMeters, fx, fy, cx, cy, numIterations);
+    }
+}

photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetectorParams.java

@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.apriltag;
+
+import java.util.Objects;
+
+public class AprilTagDetectorParams {
+    public static AprilTagDetectorParams DEFAULT_36H11 =
+            new AprilTagDetectorParams(AprilTagFamily.kTag36h11, 1.0, 0.0, 4, false, false);
+
+    public final AprilTagFamily tagFamily;
+    public final double decimate;
+    public final double blur;
+    public final int threads;
+    public final boolean debug;
+    public final boolean refineEdges;
+
+    public AprilTagDetectorParams(
+            AprilTagFamily tagFamily,
+            double decimate,
+            double blur,
+            int threads,
+            boolean debug,
+            boolean refineEdges) {
+        this.tagFamily = tagFamily;
+        this.decimate = decimate;
+        this.blur = blur;
+        this.threads = threads;
+        this.debug = debug;
+        this.refineEdges = refineEdges;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        AprilTagDetectorParams that = (AprilTagDetectorParams) o;
+        return Objects.equals(tagFamily, that.tagFamily)
+                && Double.compare(decimate, that.decimate) == 0
+                && Double.compare(blur, that.blur) == 0
+                && threads == that.threads
+                && debug == that.debug
+                && refineEdges == that.refineEdges;
+    }
+
+    @Override
+    public String toString() {
+        return "AprilTagDetectorParams{"
+                + "tagFamily="
+                + tagFamily.getNativeName()
+                + ", decimate="
+                + decimate
+                + ", blur="
+                + blur
+                + ", threads="
+                + threads
+                + ", debug="
+                + debug
+                + ", refineEdges="
+                + refineEdges
+                + '}';
+    }
+}

photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java

@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.apriltag;
+
+public enum AprilTagFamily {
+    kTag36h11,
+    kTag25h9,
+    kTag16h5,
+    kTagCircle21h7,
+    kTagCircle49h12,
+    kTagStandard41h12,
+    kTagStandard52h13,
+    kTagCustom48h11;
+
+    public String getNativeName() {
+        // We wanna strip the leading kT and replace with "t"
+        return this.name().replaceFirst("kT", "t");
+    }
+}

photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagJNI.java

@@ -0,0 +1,182 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.apriltag;
+
+import edu.wpi.first.util.RuntimeDetector;
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.File;
+import java.io.IOException;
+import java.io.InputStream;
+import java.net.URL;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import org.opencv.core.Mat;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class AprilTagJNI {
+    static final boolean USE_DEBUG =
+            false; // Development flag - should be false on release, but flip to True to read in a debug
+    // version of the library
+    static final String NATIVE_DEBUG_LIBRARY_NAME = "apriltagd";
+    static final String NATIVE_RELEASE_LIBRARY_NAME = "apriltag";
+
+    static boolean s_libraryLoaded = false;
+    static RuntimeLoader<AprilTagJNI> s_loader = null;
+    private static Logger logger = new Logger(AprilTagJNI.class, LogGroup.VisionModule);
+
+    public static synchronized void forceLoad() throws IOException {
+        if (s_libraryLoaded) return;
+
+        try {
+            // Ensure the lib directory has been created to receive the unpacked shared object
+            File libDirectory = Path.of("lib/").toFile();
+            if (!libDirectory.exists()) {
+                Files.createDirectory(libDirectory.toPath()).toFile();
+            }
+
+            // Pick the proper library based on development flags
+            String libBaseName = USE_DEBUG ? NATIVE_DEBUG_LIBRARY_NAME : NATIVE_RELEASE_LIBRARY_NAME;
+            String libFileName = System.mapLibraryName(libBaseName);
+            File libFile = Path.of("lib/" + libFileName).toFile();
+
+            // Always extract the library fresh
+            // Yes, technically, a hashing strategy should speed this up, but it's only a
+            // one-time, at-startup time hit. And not very big.
+            URL resourceURL;
+
+            String subfolder;
+            // TODO 64-bit Pi support
+            if (RuntimeDetector.isAthena()) {
+                subfolder = "athena";
+            } else if (RuntimeDetector.isAarch64()) {
+                subfolder = "aarch64";
+            } else if (RuntimeDetector.isRaspbian()) {
+                subfolder = "raspbian";
+            } else if (RuntimeDetector.isWindows()) {
+                subfolder = "win64";
+            } else if (RuntimeDetector.isLinux()) {
+                subfolder = "linux64";
+            } else if (RuntimeDetector.isMac()) {
+                subfolder = "mac";
+            } // NOT m1, afaict, lol
+            else {
+                logger.error("Could not determine platform! Cannot load Apriltag JNI");
+                return;
+            }
+
+            resourceURL =
+                    AprilTagJNI.class.getResource(
+                            "/nativelibraries/apriltag/" + subfolder + "/" + libFileName);
+
+            try (InputStream in = resourceURL.openStream()) {
+                // Remove the file if it already exists
+                if (libFile.exists()) Files.delete(libFile.toPath());
+                // Copy in a fresh resource
+                Files.copy(in, libFile.toPath());
+            }
+
+            // Actually load the library
+            System.load(libFile.getAbsolutePath());
+
+            s_libraryLoaded = true;
+
+        } catch (UnsatisfiedLinkError e) {
+            logger.error("Couldn't load apriltag shared object");
+            e.printStackTrace();
+        } catch (IOException ioe) {
+            logger.error("IO exception copying apriltag shared object");
+            ioe.printStackTrace();
+        }
+
+        if (!s_libraryLoaded) {
+            logger.error("Failed to load AprilTag Native Library!");
+        } else {
+            logger.info("AprilTag Native Library loaded successfully");
+        }
+    }
+
+    // Returns a pointer to a apriltag_detector_t
+    public static native long AprilTag_Create(
+            String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges);
+
+    // Destroy and free a previously created detector.
+    public static native long AprilTag_Destroy(long detector);
+
+    private static native Object[] AprilTag_Detect(
+            long detector,
+            long imgAddr,
+            int rows,
+            int cols,
+            boolean doPoseEstimation,
+            double tagWidth,
+            double fx,
+            double fy,
+            double cx,
+            double cy,
+            int nIters);
+
+    // Detect targets given a GRAY frame. Returns a pointer toa zarray
+    public static DetectionResult[] AprilTag_Detect(
+            long detector,
+            Mat img,
+            boolean doPoseEstimation,
+            double tagWidth,
+            double fx,
+            double fy,
+            double cx,
+            double cy,
+            int nIters) {
+        return (DetectionResult[])
+                AprilTag_Detect(
+                        detector,
+                        img.dataAddr(),
+                        img.rows(),
+                        img.cols(),
+                        doPoseEstimation,
+                        tagWidth,
+                        fx,
+                        fy,
+                        cx,
+                        cy,
+                        nIters);
+    }
+
+    public static void main(String[] args) {
+        // System.loadLibrary("apriltag");
+
+        long detector = AprilTag_Create("tag36h11", 2, 2, 1, false, true);
+
+        // var buff = ByteBuffer.allocateDirect(1280 * 720);
+
+        // // try {
+        // //     CameraServerCvJNI.forceLoad();
+        // // } catch (IOException e) {
+        // //     // TODO Auto-generated catch block
+        // //     e.printStackTrace();
+        // // }
+        // // PicamJNI.forceLoad();
+        // // TestUtils.loadLibraries();
+        // var img = Imgcodecs.imread("~/Downloads/TagFams.jpg");
+
+        // var ret = AprilTag_Detect(detector, 0, 720, 1280);
+        // System.out.println(detector);
+        // System.out.println(ret);
+        // System.out.println(List.of(ret));
+    }
+}

photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java

@@ -0,0 +1,164 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.apriltag;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.util.Arrays;
+
+public class DetectionResult {
+    public int getId() {
+        return id;
+    }
+
+    public int getHamming() {
+        return hamming;
+    }
+
+    public float getDecisionMargin() {
+        return decision_margin;
+    }
+
+    public void setDecisionMargin(float decision_margin) {
+        this.decision_margin = decision_margin;
+    }
+
+    public double[] getHomography() {
+        return homography;
+    }
+
+    public void setHomography(double[] homography) {
+        this.homography = homography;
+    }
+
+    public double getCenterX() {
+        return centerX;
+    }
+
+    public void setCenterX(double centerX) {
+        this.centerX = centerX;
+    }
+
+    public double getCenterY() {
+        return centerY;
+    }
+
+    public void setCenterY(double centerY) {
+        this.centerY = centerY;
+    }
+
+    public double[] getCorners() {
+        return corners;
+    }
+
+    public void setCorners(double[] corners) {
+        this.corners = corners;
+    }
+
+    public double getError1() {
+        return error1;
+    }
+
+    public double getError2() {
+        return error2;
+    }
+
+    public Pose3d getPoseResult1() {
+        return poseResult1;
+    }
+
+    public Pose3d getPoseResult2() {
+        return poseResult2;
+    }
+
+    int id;
+    int hamming;
+    float decision_margin;
+    double[] homography;
+    double centerX, centerY;
+    double[] corners;
+
+    Pose3d poseResult1;
+    double error1;
+    Pose3d poseResult2;
+    double error2;
+
+    public DetectionResult(
+            int id,
+            int hamming,
+            float decision_margin,
+            double[] homography,
+            double centerX,
+            double centerY,
+            double[] corners,
+            double[] pose1TransArr,
+            double[] pose1RotArr,
+            double err1,
+            double[] pose2TransArr,
+            double[] pose2RotArr,
+            double err2) {
+        this.id = id;
+        this.hamming = hamming;
+        this.decision_margin = decision_margin;
+        this.homography = homography;
+        this.centerX = centerX;
+        this.centerY = centerY;
+        this.corners = corners;
+
+        this.error1 = err1;
+        this.poseResult1 =
+                new Pose3d(
+                        new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]),
+                        new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr)));
+        this.error2 = err2;
+        this.poseResult2 =
+                new Pose3d(
+                        new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]),
+                        new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr)));
+    }
+
+    @Override
+    public String toString() {
+        return "DetectionResult [centerX="
+                + centerX
+                + ", centerY="
+                + centerY
+                + ", corners="
+                + Arrays.toString(corners)
+                + ", decision_margin="
+                + decision_margin
+                + ", error1="
+                + error1
+                + ", error2="
+                + error2
+                + ", hamming="
+                + hamming
+                + ", homography="
+                + Arrays.toString(homography)
+                + ", id="
+                + id
+                + ", poseResult1="
+                + poseResult1
+                + ", poseResult2="
+                + poseResult2
+                + "]";
+    }
+}

photon-core/src/main/java/org/photonvision/vision/camera/CameraQuirk.java

@@ -23,5 +23,9 @@ public enum CameraQuirk {
     /** For the Raspberry Pi Camera */
     PiCam,
     /** Cap at 100FPS for high-bandwidth cameras */
-    FPSCap100
+    FPSCap100,
+    /** Separate red/blue gain controls available */
+    AWBGain,
+    /** Will not work with photonvision - Logitec C270 at least */
+    CompletelyBroken
 }

photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java

@@ -43,7 +43,6 @@ public class FileVisionSource extends VisionSource {
                         Path.of(cameraConfiguration.path),
                         cameraConfiguration.FOV,
                         FileFrameProvider.MAX_FPS,
-                        cameraConfiguration.camPitch,
                         calibration);
         settables =
                 new FileSourceSettables(cameraConfiguration, frameProvider.get().frameStaticProperties);
@@ -98,6 +97,9 @@ public class FileVisionSource extends VisionSource {
         @Override
         public void setGain(int gain) {}
 
+        @Override
+        public void setLowExposureOptimization(boolean mode) {}
+
         @Override
         public VideoMode getCurrentVideoMode() {
             return videoMode;

photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java

@@ -24,8 +24,14 @@ import java.util.Objects;
 public class QuirkyCamera {
     private static final List<QuirkyCamera> quirkyCameras =
             List.of(
+                    new QuirkyCamera(
+                            0x9331,
+                            0x5A3,
+                            CameraQuirk.CompletelyBroken), // Chris's older generic "Logitec HD Webcam"
+                    new QuirkyCamera(0x825, 0x46D, CameraQuirk.CompletelyBroken), // Logitec C270
                     new QuirkyCamera(0x2000, 0x1415, CameraQuirk.Gain, CameraQuirk.FPSCap100), // PS3Eye
-                    new QuirkyCamera(-1, -1, "mmal service 16.1", CameraQuirk.PiCam) // PiCam (via V4L2)
+                    new QuirkyCamera(
+                            -1, -1, "mmal service 16.1", CameraQuirk.PiCam) // PiCam (via V4L2, not zerocopy)
                     );
 
     public static final QuirkyCamera DefaultCamera = new QuirkyCamera(0, 0, "");
@@ -35,7 +41,8 @@ public class QuirkyCamera {
                     -1,
                     "mmal service 16.1",
                     CameraQuirk.PiCam,
-                    CameraQuirk.Gain); // PiCam (special zerocopy version)
+                    CameraQuirk.Gain,
+                    CameraQuirk.AWBGain); // PiCam (special zerocopy version)
 
     public final String baseName;
     public final int usbVid;

photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java

@@ -44,6 +44,7 @@ public class USBCameraSource extends VisionSource {
 
     public USBCameraSource(CameraConfiguration config) {
         super(config);
+
         logger = new Logger(USBCameraSource.class, config.nickname, LogGroup.Camera);
         camera = new UsbCamera(config.nickname, config.path);
         cvSink = CameraServer.getInstance().getVideo(this.camera);
@@ -56,19 +57,68 @@ public class USBCameraSource extends VisionSource {
             logger.info("Quirky camera detected: " + cameraQuirks.baseName);
         }
 
-        usbCameraSettables = new USBCameraSettables(config);
-        usbFrameProvider = new USBFrameProvider(cvSink, usbCameraSettables);
+        if (cameraQuirks.hasQuirk(CameraQuirk.CompletelyBroken)) {
+            // set some defaults, as these should never be used.
+            logger.info("Camera " + cameraQuirks.baseName + " is not supported for PhotonVision");
+            usbCameraSettables = null;
+            usbFrameProvider = null;
+        } else {
+            // Normal init
+            setLowExposureOptimizationImpl(false);
+            usbCameraSettables = new USBCameraSettables(config);
+            usbFrameProvider = new USBFrameProvider(cvSink, usbCameraSettables);
+        }
+    }
 
+    void setLowExposureOptimizationImpl(boolean lowExposureMode) {
         if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
-            // Pick a bunch of reasonable setting defaults for vision processing.
-            camera.getProperty("exposure_dynamic_framerate").set(0);
-            camera.getProperty("auto_exposure_bias").set(0);
-            camera.getProperty("image_stabilization").set(0);
-            camera.getProperty("iso_sensitivity").set(0);
-            camera.getProperty("iso_sensitivity_auto").set(0);
+            // Case, we know this is a picam. Go through v4l2-ctl interface directly
+
+            // Common settings
+            camera
+                    .getProperty("image_stabilization")
+                    .set(0); // No image stabilization, as this will throw off odometry
+            camera.getProperty("power_line_frequency").set(2); // Assume 60Hz USA
+            camera.getProperty("scene_mode").set(0); // no presets
             camera.getProperty("exposure_metering_mode").set(0);
-            camera.getProperty("scene_mode").set(0);
-            camera.getProperty("power_line_frequency").set(2);
+            camera.getProperty("exposure_dynamic_framerate").set(0);
+
+            if (lowExposureMode) {
+                // Pick a bunch of reasonable setting defaults for vision processing retroreflective
+                camera.getProperty("auto_exposure_bias").set(0);
+                camera.getProperty("iso_sensitivity_auto").set(0); // Disable auto ISO adjustement
+                camera.getProperty("iso_sensitivity").set(0); // Manual ISO adjustement
+                camera.getProperty("white_balance_auto_preset").set(2); // Auto white-balance disabled
+                camera.getProperty("auto_exposure").set(1); // auto exposure disabled
+            } else {
+                // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise
+                // nice-for-humans
+                camera.getProperty("auto_exposure_bias").set(12);
+                camera.getProperty("iso_sensitivity_auto").set(1);
+                camera.getProperty("iso_sensitivity").set(1); // Manual ISO adjustement by default
+                camera.getProperty("white_balance_auto_preset").set(1); // Auto white-balance enabled
+                camera.getProperty("auto_exposure").set(0); // auto exposure enabled
+            }
+
+        } else {
+            // Case - this is some other USB cam. Default to wpilib's implementation
+
+            var canSetWhiteBalance = !cameraQuirks.hasQuirk(CameraQuirk.Gain);
+
+            if (lowExposureMode) {
+                // Pick a bunch of reasonable setting defaults for vision processing retroreflective
+                if (canSetWhiteBalance) {
+                    camera.setWhiteBalanceManual(4000); // Auto white-balance disabled, 4000K preset
+                }
+                this.getSettables().setExposure(50); // auto exposure disabled, put a sane default
+            } else {
+                // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise
+                // nice-for-humans
+                if (canSetWhiteBalance) {
+                    camera.setWhiteBalanceAuto(); // Auto white-balance enabled
+                }
+                camera.setExposureAuto(); // auto exposure enabled
+            }
         }
     }
 
@@ -89,43 +139,50 @@ public class USBCameraSource extends VisionSource {
             setVideoMode(videoModes.get(0));
         }
 
-        private int timeToPiCamV2RawExposure(double time_us) {
+        private int timeToPiCamRawExposure(double time_us) {
             int retVal =
-                    (int) Math.round(time_us / 100.0); // PiCamV2 needs exposure time in units of 100us/bit
+                    (int)
+                            Math.round(
+                                    time_us / 100.0); // Pi Cam's (both v1 and v2) need exposure time in units of
+            // 100us/bit
             return Math.min(Math.max(retVal, 1), 10000); // Cap to allowable range for parameter
         }
 
         private double pctToExposureTimeUs(double pct_in) {
             // Mirror the photonvision raspicam driver's algorithm for picking an exposure time
             // from a 0-100% input
-            final double PADDING_LOW_US = 100;
-            final double PADDING_HIGH_US = 200;
+            final double PADDING_LOW_US = 10;
+            final double PADDING_HIGH_US = 10;
             return PADDING_LOW_US
                     + (pct_in / 100.0) * ((1e6 / (double) camera.getVideoMode().fps) - PADDING_HIGH_US);
         }
 
+        @Override
+        public void setLowExposureOptimization(boolean mode) {
+            setLowExposureOptimizationImpl(mode);
+        }
+
         @Override
         public void setExposure(double exposure) {
-            try {
-                int scaledExposure = 1;
-                if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
-                    camera.getProperty("white_balance_auto_preset").set(2); // Auto white-balance off
-                    camera.getProperty("auto_exposure").set(1); // auto exposure off
+            if (exposure >= 0.0) {
+                try {
+                    int scaledExposure = 1;
+                    if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
+                        scaledExposure =
+                                (int) Math.round(timeToPiCamRawExposure(pctToExposureTimeUs(exposure)));
+                        logger.debug("Setting camera raw exposure to " + Integer.toString(scaledExposure));
+                        camera.getProperty("raw_exposure_time_absolute").set(scaledExposure);
+                        camera.getProperty("raw_exposure_time_absolute").set(scaledExposure);
 
-                    scaledExposure =
-                            (int) Math.round(timeToPiCamV2RawExposure(pctToExposureTimeUs(exposure)));
-                    logger.debug("Setting camera raw exposure to " + Integer.toString(scaledExposure));
-                    camera.getProperty("raw_exposure_time_absolute").set(scaledExposure);
-                    camera.getProperty("raw_exposure_time_absolute").set(scaledExposure);
-
-                } else {
-                    scaledExposure = (int) Math.round(exposure);
-                    logger.debug("Setting camera exposure to " + Integer.toString(scaledExposure));
-                    camera.setExposureManual(scaledExposure);
-                    camera.setExposureManual(scaledExposure);
+                    } else {
+                        scaledExposure = (int) Math.round(exposure);
+                        logger.debug("Setting camera exposure to " + Integer.toString(scaledExposure));
+                        camera.setExposureManual(scaledExposure);
+                        camera.setExposureManual(scaledExposure);
+                    }
+                } catch (VideoException e) {
+                    logger.error("Failed to set camera exposure!", e);
                 }
-            } catch (VideoException e) {
-                logger.error("Failed to set camera exposure!", e);
             }
         }
 
@@ -180,8 +237,16 @@ public class USBCameraSource extends VisionSource {
                         modes =
                                 new VideoMode[] {
                                     new VideoMode(VideoMode.PixelFormat.kBGR, 320, 240, 90),
+                                    new VideoMode(VideoMode.PixelFormat.kBGR, 320, 240, 30),
+                                    new VideoMode(VideoMode.PixelFormat.kBGR, 320, 240, 15),
+                                    new VideoMode(VideoMode.PixelFormat.kBGR, 320, 240, 10),
                                     new VideoMode(VideoMode.PixelFormat.kBGR, 640, 480, 90),
+                                    new VideoMode(VideoMode.PixelFormat.kBGR, 640, 480, 45),
+                                    new VideoMode(VideoMode.PixelFormat.kBGR, 640, 480, 30),
+                                    new VideoMode(VideoMode.PixelFormat.kBGR, 640, 480, 15),
+                                    new VideoMode(VideoMode.PixelFormat.kBGR, 640, 480, 10),
                                     new VideoMode(VideoMode.PixelFormat.kBGR, 960, 720, 60),
+                                    new VideoMode(VideoMode.PixelFormat.kBGR, 960, 720, 10),
                                     new VideoMode(VideoMode.PixelFormat.kBGR, 1280, 720, 45),
                                     new VideoMode(VideoMode.PixelFormat.kBGR, 1920, 1080, 20),
                                 };
@@ -212,21 +277,24 @@ public class USBCameraSource extends VisionSource {
 
                         videoModesList.add(videoMode);
 
+                        // TODO - do we want to trim down FPS modes? in cases where the camera has no gain
+                        // control,
+                        // lower FPS might be needed to ensure total exposure is acceptable.
                         // We look for modes with the same height/width/pixelformat as this mode
                         // and remove all the ones that are slower. This is sorted low to high.
                         // So we remove the last element (the fastest FPS) from the duplicate list,
                         // and remove all remaining elements from the final list
-                        var duplicateModes =
-                                videoModesList.stream()
-                                        .filter(
-                                                it ->
-                                                        it.height == videoMode.height
-                                                                && it.width == videoMode.width
-                                                                && it.pixelFormat == videoMode.pixelFormat)
-                                        .sorted(Comparator.comparingDouble(it -> it.fps))
-                                        .collect(Collectors.toList());
-                        duplicateModes.remove(duplicateModes.size() - 1);
-                        videoModesList.removeAll(duplicateModes);
+                        // var duplicateModes =
+                        //         videoModesList.stream()
+                        //                 .filter(
+                        //                         it ->
+                        //                                 it.height == videoMode.height
+                        //                                         && it.width == videoMode.width
+                        //                                         && it.pixelFormat == videoMode.pixelFormat)
+                        //                 .sorted(Comparator.comparingDouble(it -> it.fps))
+                        //                 .collect(Collectors.toList());
+                        // duplicateModes.remove(duplicateModes.size() - 1);
+                        // videoModesList.removeAll(duplicateModes);
                     }
                 } catch (Exception e) {
                     logger.error("Exception while enumerating video modes!", e);

photon-core/src/main/java/org/photonvision/vision/camera/ZeroCopyPicamSource.java

@@ -45,6 +45,13 @@ public class ZeroCopyPicamSource extends VisionSource {
 
         settables = new PicamSettables(configuration);
         frameProvider = new AcceleratedPicamFrameProvider(settables);
+
+        setLowExposureOptimizationImpl(false);
+    }
+
+    static void setLowExposureOptimizationImpl(boolean mode) {
+        // TODO - ZeroCopy does not... yet? ... have the configuration params necessary to make this
+        // work well.
     }
 
     @Override
@@ -101,10 +108,14 @@ public class ZeroCopyPicamSource extends VisionSource {
                 videoModes.put(
                         0, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 320, 240, 120, 120, .39));
                 videoModes.put(
-                        1, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 640, 480, 65, 90, .39));
+                        1, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 320, 240, 30, 30, .39));
+                videoModes.put(
+                        2, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 640, 480, 65, 90, .39));
+                videoModes.put(
+                        3, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 640, 480, 30, 30, .39));
                 // TODO: fix 1280x720 in the native code and re-add it
                 videoModes.put(
-                        3, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1920, 1080, 15, 20, .53));
+                        4, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1920, 1080, 15, 20, .53));
             } else {
                 if (sensorModel == PicamJNI.SensorModel.IMX477) {
                     logger.warn(
@@ -118,13 +129,17 @@ public class ZeroCopyPicamSource extends VisionSource {
                 videoModes.put(
                         0, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 320, 240, 90, 90, 1));
                 videoModes.put(
-                        1, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 640, 480, 85, 90, 1));
+                        1, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 320, 240, 30, 30, 1));
                 videoModes.put(
-                        2, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 960, 720, 45, 49, 0.74));
+                        2, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 640, 480, 85, 90, 1));
                 videoModes.put(
-                        3, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1280, 720, 30, 45, 0.91));
+                        3, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 640, 480, 30, 30, 1));
                 videoModes.put(
-                        4, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1920, 1080, 15, 20, 0.72));
+                        4, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 960, 720, 45, 49, 0.74));
+                videoModes.put(
+                        5, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1280, 720, 30, 45, 0.91));
+                videoModes.put(
+                        6, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1920, 1080, 15, 20, 0.72));
             }
 
             currentVideoMode = (FPSRatedVideoMode) videoModes.get(0);
@@ -137,11 +152,21 @@ public class ZeroCopyPicamSource extends VisionSource {
 
         @Override
         public void setExposure(double exposure) {
+            // Todo - for now, handle auto exposure by using 100% exposure
+            if (exposure < 0.0) {
+                exposure = 100.0;
+            }
+
             lastExposure = exposure;
             var failure = PicamJNI.setExposure((int) Math.round(exposure));
             if (failure) logger.warn("Couldn't set Pi Camera exposure");
         }
 
+        @Override
+        public void setLowExposureOptimization(boolean mode) {
+            setLowExposureOptimizationImpl(mode);
+        }
+
         @Override
         public void setBrightness(int brightness) {
             lastBrightness = brightness;

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

@@ -17,7 +17,9 @@
 
 package org.photonvision.vision.frame;
 
-import edu.wpi.first.math.geometry.Rotation2d;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.Releasable;
@@ -38,10 +40,14 @@ public class Frame implements Releasable {
     }
 
     public Frame() {
-        this(
-                new CVMat(),
+        this(new CVMat(), MathUtils.wpiNanoTime(), new FrameStaticProperties(0, 0, 0, null));
+    }
+
+    public static Frame emptyFrame(int width, int height) {
+        return new Frame(
+                new CVMat(Mat.zeros(new Size(width, height), CvType.CV_8UC3)),
                 MathUtils.wpiNanoTime(),
-                new FrameStaticProperties(0, 0, 0, new Rotation2d(), null));
+                new FrameStaticProperties(width, height, 0, null));
     }
 
     public void copyTo(Frame destFrame) {

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

@@ -18,7 +18,6 @@
 package org.photonvision.vision.frame;
 
 import edu.wpi.first.cscore.VideoMode;
-import edu.wpi.first.math.geometry.Rotation2d;
 import org.opencv.core.Point;
 import org.photonvision.common.util.numbers.DoubleCouple;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
@@ -34,7 +33,6 @@ public class FrameStaticProperties {
     public final Point centerPoint;
     public final double horizontalFocalLength;
     public final double verticalFocalLength;
-    public final Rotation2d cameraPitch;
     public CameraCalibrationCoefficients cameraCalibration;
 
     /**
@@ -43,9 +41,8 @@ public class FrameStaticProperties {
      * @param mode The Video Mode of the camera.
      * @param fov The fov of the image.
      */
-    public FrameStaticProperties(
-            VideoMode mode, double fov, Rotation2d cameraPitch, CameraCalibrationCoefficients cal) {
-        this(mode != null ? mode.width : 1, mode != null ? mode.height : 1, fov, cameraPitch, cal);
+    public FrameStaticProperties(VideoMode mode, double fov, CameraCalibrationCoefficients cal) {
+        this(mode != null ? mode.width : 1, mode != null ? mode.height : 1, fov, cal);
     }
 
     /**
@@ -56,15 +53,10 @@ public class FrameStaticProperties {
      * @param fov The fov of the image.
      */
     public FrameStaticProperties(
-            int imageWidth,
-            int imageHeight,
-            double fov,
-            Rotation2d cameraPitch,
-            CameraCalibrationCoefficients cal) {
+            int imageWidth, int imageHeight, double fov, CameraCalibrationCoefficients cal) {
         this.imageWidth = imageWidth;
         this.imageHeight = imageHeight;
         this.fov = fov;
-        this.cameraPitch = cameraPitch;
         this.cameraCalibration = cal;
 
         imageArea = this.imageWidth * this.imageHeight;

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

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.frame.provider;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import java.nio.file.Files;
 import java.nio.file.Path;
 import java.nio.file.Paths;
@@ -34,7 +33,7 @@ import org.photonvision.vision.opencv.CVMat;
  * path}.
  */
 public class FileFrameProvider implements FrameProvider {
-    public static final int MAX_FPS = 10;
+    public static final int MAX_FPS = 5;
     private static int count = 0;
 
     private final int thisIndex = count++;
@@ -54,20 +53,15 @@ public class FileFrameProvider implements FrameProvider {
      * @param maxFPS The max framerate to provide the image at.
      */
     public FileFrameProvider(Path path, double fov, int maxFPS) {
-        this(path, fov, maxFPS, null, null);
+        this(path, fov, maxFPS, null);
+    }
+
+    public FileFrameProvider(Path path, double fov, CameraCalibrationCoefficients calibration) {
+        this(path, fov, MAX_FPS, calibration);
     }
 
     public FileFrameProvider(
-            Path path, double fov, Rotation2d pitch, CameraCalibrationCoefficients calibration) {
-        this(path, fov, MAX_FPS, pitch, calibration);
-    }
-
-    public FileFrameProvider(
-            Path path,
-            double fov,
-            int maxFPS,
-            Rotation2d pitch,
-            CameraCalibrationCoefficients calibration) {
+            Path path, double fov, int maxFPS, CameraCalibrationCoefficients calibration) {
         if (!Files.exists(path))
             throw new RuntimeException("Invalid path for image: " + path.toAbsolutePath().toString());
         this.path = path;
@@ -75,8 +69,7 @@ public class FileFrameProvider implements FrameProvider {
 
         Mat rawImage = Imgcodecs.imread(path.toString());
         if (rawImage.cols() > 0 && rawImage.rows() > 0) {
-            properties =
-                    new FrameStaticProperties(rawImage.width(), rawImage.height(), fov, pitch, calibration);
+            properties = new FrameStaticProperties(rawImage.width(), rawImage.height(), fov, calibration);
             originalFrame = new Frame(new CVMat(rawImage), properties);
         } else {
             throw new RuntimeException("Image loading failed!");

photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java

@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import java.util.List;
+import org.opencv.core.Mat;
+import org.photonvision.vision.apriltag.AprilTagDetector;
+import org.photonvision.vision.apriltag.DetectionResult;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class AprilTagDetectionPipe
+        extends CVPipe<Mat, List<DetectionResult>, AprilTagDetectionPipeParams> {
+    private final AprilTagDetector m_detector = new AprilTagDetector();
+
+    boolean useNativePoseEst;
+
+    @Override
+    protected List<DetectionResult> process(Mat in) {
+        return List.of(
+                m_detector.detect(
+                        in,
+                        params.cameraCalibrationCoefficients,
+                        useNativePoseEst,
+                        params.numIterations,
+                        params.tagWidthMeters));
+    }
+
+    @Override
+    public void setParams(AprilTagDetectionPipeParams params) {
+        super.setParams(params);
+        m_detector.updateParams(params.detectorParams);
+    }
+
+    public void setNativePoseEstimationEnabled(boolean enabled) {
+        this.useNativePoseEst = enabled;
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java

@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import java.util.Objects;
+import org.photonvision.vision.apriltag.AprilTagDetectorParams;
+import org.photonvision.vision.apriltag.AprilTagFamily;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+
+public class AprilTagDetectionPipeParams {
+    public final AprilTagDetectorParams detectorParams;
+    public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
+    public final int numIterations;
+    public final double tagWidthMeters;
+
+    public AprilTagDetectionPipeParams(
+            AprilTagFamily tagFamily,
+            double decimate,
+            double blur,
+            int threads,
+            boolean debug,
+            boolean refineEdges,
+            int numIters,
+            double tagWidthMeters,
+            CameraCalibrationCoefficients cameraCalibrationCoefficients) {
+        detectorParams =
+                new AprilTagDetectorParams(tagFamily, decimate, blur, threads, debug, refineEdges);
+        this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
+        this.numIterations = numIters;
+        this.tagWidthMeters = tagWidthMeters;
+    }
+
+    public AprilTagDetectionPipeParams(
+            AprilTagDetectorParams detectorParams,
+            CameraCalibrationCoefficients cameraCalibrationCoefficients,
+            int numIters,
+            double tagWidthMeters) {
+        this.detectorParams = detectorParams;
+        this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
+        this.numIterations = numIters;
+        this.tagWidthMeters = tagWidthMeters;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        AprilTagDetectionPipeParams that = (AprilTagDetectionPipeParams) o;
+        return Objects.equals(detectorParams, that.detectorParams)
+                && Objects.equals(cameraCalibrationCoefficients, that.cameraCalibrationCoefficients);
+    }
+
+    @Override
+    public String toString() {
+        return "AprilTagDetectionPipeParams{"
+                + "detectorParams="
+                + detectorParams
+                + ", cameraCalibrationCoefficients="
+                + cameraCalibrationCoefficients
+                + '}';
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dAprilTagsPipe.java

@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import java.awt.*;
+import org.photonvision.vision.frame.FrameDivisor;
+
+public class Draw2dAprilTagsPipe extends Draw2dTargetsPipe {
+    public static class Draw2dAprilTagsParams extends Draw2dTargetsPipe.Draw2dTargetsParams {
+        public Draw2dAprilTagsParams(
+                boolean shouldDraw, boolean showMultipleTargets, FrameDivisor divisor) {
+            super(shouldDraw, showMultipleTargets, divisor);
+            // We want to show the polygon, not the rotated box
+            this.showRotatedBox = false;
+            this.showMaximumBox = false;
+            this.rotatedBoxColor = Color.RED;
+        }
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java

@@ -97,11 +97,15 @@ public class Draw2dTargetsPipe
                     if (poly == null && target.getShape() != null)
                         poly = target.getShape().getContour().getApproxPolyDp();
                     if (poly != null) {
-                        //                        divideMat2f(poly, pointMat);
                         var mat = new MatOfPoint();
                         mat.fromArray(poly.toArray());
+                        divideMat(mat, mat);
                         Imgproc.drawContours(
-                                in.getLeft(), List.of(mat), -1, ColorHelper.colorToScalar(Color.blue), 2);
+                                in.getLeft(),
+                                List.of(mat),
+                                -1,
+                                ColorHelper.colorToScalar(params.rotatedBoxColor),
+                                2);
                         mat.release();
                     }
                 }
@@ -134,9 +138,12 @@ public class Draw2dTargetsPipe
                                     center.y - params.kPixelsToOffset * imageSize);
                     dividePoint(textPos);
 
+                    int id = target.getFiducialId();
+                    var contourNumber = String.valueOf(id == -1 ? i : id);
+
                     Imgproc.putText(
                             in.getLeft(),
-                            String.valueOf(i),
+                            contourNumber,
                             textPos,
                             0,
                             textSize,
@@ -182,6 +189,14 @@ public class Draw2dTargetsPipe
         dst.fromArray(hull);
     }
 
+    private void divideMat(MatOfPoint2f src, MatOfPoint dst) {
+        var hull = src.toArray();
+        for (Point point : hull) {
+            dividePoint(point);
+        }
+        dst.fromArray(hull);
+    }
+
     /** Scale a given point list by the current frame divisor. the point list is mutated! */
     private void dividePointList(List<Point> points) {
         for (var p : points) {

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dAprilTagsPipe.java

@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+import org.photonvision.vision.frame.FrameDivisor;
+import org.photonvision.vision.target.TargetModel;
+
+public class Draw3dAprilTagsPipe extends Draw3dTargetsPipe {
+    public static class Draw3dAprilTagsParams extends Draw3dContoursParams {
+        public Draw3dAprilTagsParams(
+                boolean shouldDraw,
+                CameraCalibrationCoefficients cameraCalibrationCoefficients,
+                TargetModel targetModel,
+                FrameDivisor divisor) {
+            super(shouldDraw, cameraCalibrationCoefficients, targetModel, divisor);
+            this.shouldDrawHull = false;
+        }
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java

@@ -18,6 +18,7 @@
 package org.photonvision.vision.pipe.impl;
 
 import java.awt.*;
+import java.util.ArrayList;
 import java.util.List;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.calib3d.Calib3d;
@@ -45,31 +46,38 @@ public class Draw3dTargetsPipe
 
         for (var target : in.getRight()) {
             // draw convex hull
-            var pointMat = new MatOfPoint();
-            divideMat2f(target.m_mainContour.getConvexHull(), pointMat);
-            if (pointMat.size().empty()) {
-                logger.error("Convex hull is empty?");
-                logger.debug(
-                        "Orig. Convex Hull: " + target.m_mainContour.getConvexHull().size().toString());
-                continue;
-            }
-            Imgproc.drawContours(
-                    in.getLeft(), List.of(pointMat), -1, ColorHelper.colorToScalar(Color.green), 1);
-
-            // draw approximate polygon
-            var poly = target.getApproximateBoundingPolygon();
-            if (poly != null) {
-                divideMat2f(poly, pointMat);
+            if (params.shouldDrawHull(target)) {
+                var pointMat = new MatOfPoint();
+                divideMat2f(target.m_mainContour.getConvexHull(), pointMat);
+                if (pointMat.size().empty()) {
+                    logger.error("Convex hull is empty?");
+                    logger.debug(
+                            "Orig. Convex Hull: " + target.m_mainContour.getConvexHull().size().toString());
+                    continue;
+                }
                 Imgproc.drawContours(
-                        in.getLeft(), List.of(pointMat), -1, ColorHelper.colorToScalar(Color.blue), 2);
+                        in.getLeft(), List.of(pointMat), -1, ColorHelper.colorToScalar(Color.green), 1);
+
+                // draw approximate polygon
+                var poly = target.getApproximateBoundingPolygon();
+                if (poly != null) {
+                    divideMat2f(poly, pointMat);
+                    Imgproc.drawContours(
+                            in.getLeft(), List.of(pointMat), -1, ColorHelper.colorToScalar(Color.blue), 2);
+                }
+                pointMat.release();
             }
 
             // Draw floor and top
-            if (target.getCameraRelativeRvec() != null && target.getCameraRelativeTvec() != null) {
+            if (target.getCameraRelativeRvec() != null
+                    && target.getCameraRelativeTvec() != null
+                    && params.shouldDrawBox) {
                 var tempMat = new MatOfPoint2f();
+
                 var jac = new Mat();
                 var bottomModel = params.targetModel.getVisualizationBoxBottom();
                 var topModel = params.targetModel.getVisualizationBoxTop();
+
                 Calib3d.projectPoints(
                         bottomModel,
                         target.getCameraRelativeRvec(),
@@ -78,7 +86,10 @@ public class Draw3dTargetsPipe
                         params.cameraCalibrationCoefficients.getCameraExtrinsicsMat(),
                         tempMat,
                         jac);
+                // Distort the points so they match the image they're being overlaid on
+                distortPoints(tempMat, tempMat);
                 var bottomPoints = tempMat.toList();
+
                 Calib3d.projectPoints(
                         topModel,
                         target.getCameraRelativeRvec(),
@@ -87,6 +98,7 @@ public class Draw3dTargetsPipe
                         params.cameraCalibrationCoefficients.getCameraExtrinsicsMat(),
                         tempMat,
                         jac);
+                distortPoints(tempMat, tempMat);
                 var topPoints = tempMat.toList();
 
                 dividePointList(bottomPoints);
@@ -120,7 +132,6 @@ public class Draw3dTargetsPipe
                 tempMat.release();
                 jac.release();
             }
-            pointMat.release();
 
             // draw corners
             var corners = target.getTargetCorners();
@@ -142,6 +153,45 @@ public class Draw3dTargetsPipe
         return null;
     }
 
+    private void distortPoints(MatOfPoint2f src, MatOfPoint2f dst) {
+        var pointsList = src.toList();
+        var dstList = new ArrayList<Point>();
+        final Mat cameraMatrix = params.cameraCalibrationCoefficients.getCameraIntrinsicsMat();
+        // k1, k2, p1, p2, k3
+        final Mat distCoeffs = params.cameraCalibrationCoefficients.getCameraExtrinsicsMat();
+        var cx = cameraMatrix.get(0, 2)[0];
+        var cy = cameraMatrix.get(1, 2)[0];
+        var fx = cameraMatrix.get(0, 0)[0];
+        var fy = cameraMatrix.get(1, 1)[0];
+        var k1 = distCoeffs.get(0, 0)[0];
+        var k2 = distCoeffs.get(0, 1)[0];
+        var k3 = distCoeffs.get(0, 4)[0];
+        var p1 = distCoeffs.get(0, 2)[0];
+        var p2 = distCoeffs.get(0, 3)[0];
+
+        for (Point point : pointsList) {
+            // To relative coordinates <- this is the step you are missing.
+            double x = (point.x - cx) / fx; // cx, cy is the center of distortion
+            double y = (point.y - cy) / fy;
+
+            double r2 = x * x + y * y; // square of the radius from center
+
+            // Radial distorsion
+            double xDistort = x * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
+            double yDistort = y * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
+
+            // Tangential distorsion
+            xDistort = xDistort + (2 * p1 * x * y + p2 * (r2 + 2 * x * x));
+            yDistort = yDistort + (p1 * (r2 + 2 * y * y) + 2 * p2 * x * y);
+
+            // Back to absolute coordinates.
+            xDistort = xDistort * fx + cx;
+            yDistort = yDistort * fy + cy;
+            dstList.add(new Point(xDistort, yDistort));
+        }
+        dst.fromList(dstList);
+    }
+
     private void divideMat2f(MatOfPoint2f src, MatOfPoint dst) {
         var hull = src.toArray();
         var pointArray = new Point[hull.length];
@@ -154,6 +204,18 @@ public class Draw3dTargetsPipe
         dst.fromArray(pointArray);
     }
 
+    private void divideMat2f(MatOfPoint2f src, MatOfPoint2f dst) {
+        var hull = src.toArray();
+        var pointArray = new Point[hull.length];
+        for (int i = 0; i < hull.length; i++) {
+            var hullAtI = hull[i];
+            pointArray[i] =
+                    new Point(
+                            hullAtI.x / (double) params.divisor.value, hullAtI.y / (double) params.divisor.value);
+        }
+        dst.fromArray(pointArray);
+    }
+
     /** Scale a given point list by the current frame divisor. the point list is mutated! */
     private void dividePointList(List<Point> points) {
         for (var p : points) {
@@ -167,6 +229,8 @@ public class Draw3dTargetsPipe
         public Color color = Color.RED;
 
         public final boolean shouldDraw;
+        public boolean shouldDrawHull = true;
+        public boolean shouldDrawBox = true;
         public final TargetModel targetModel;
         public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
         public final FrameDivisor divisor;
@@ -181,5 +245,9 @@ public class Draw3dTargetsPipe
             this.targetModel = targetModel;
             this.divisor = divisor;
         }
+
+        public boolean shouldDrawHull(TrackedTarget t) {
+            return !t.isFiducial() && this.shouldDrawHull;
+        }
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/GrayscalePipe.java

@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import org.opencv.core.Mat;
+import org.opencv.imgproc.Imgproc;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class GrayscalePipe extends CVPipe<Mat, Mat, GrayscalePipe.GrayscaleParams> {
+    @Override
+    protected Mat process(Mat in) {
+        var outputMat = new Mat();
+        // We can save a copy here by sending the output of cvtcolor to outputMat directly
+        // rather than copying. Free performance!
+        Imgproc.cvtColor(in, outputMat, Imgproc.COLOR_BGR2GRAY, 3);
+
+        return outputMat;
+    }
+
+    public static class GrayscaleParams {
+        public GrayscaleParams() {}
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java

@@ -17,9 +17,11 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import java.util.List;
 import org.opencv.calib3d.Calib3d;
 import org.opencv.core.Core;
@@ -28,6 +30,7 @@ import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Scalar;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipe.CVPipe;
 import org.photonvision.vision.target.TargetModel;
@@ -61,8 +64,6 @@ public class SolvePNPPipe
     }
 
     private void calculateTargetPose(TrackedTarget target) {
-        Transform2d targetPose;
-
         var corners = target.getTargetCorners();
         if (corners == null
                 || corners.isEmpty()
@@ -91,9 +92,16 @@ public class SolvePNPPipe
         target.setCameraRelativeTvec(tVec);
         target.setCameraRelativeRvec(rVec);
 
-        targetPose = correctLocationForCameraPitch(tVec, rVec, params.cameraPitchAngle);
+        Translation3d translation =
+                new Translation3d(tVec.get(0, 0)[0], tVec.get(1, 0)[0], tVec.get(2, 0)[0]);
+        Rotation3d rotation =
+                new Rotation3d(
+                        VecBuilder.fill(rVec.get(0, 0)[0], rVec.get(1, 0)[0], rVec.get(2, 0)[0]),
+                        Core.norm(rVec));
 
-        target.setCameraToTarget(targetPose);
+        Pose3d targetPose = MathUtils.convertOpenCVtoPhotonPose(new Transform3d(translation, rotation));
+        target.setCameraToTarget3d(
+                new Transform3d(targetPose.getTranslation(), targetPose.getRotation()));
     }
 
     Mat rotationMatrix = new Mat();
@@ -102,43 +110,6 @@ public class SolvePNPPipe
     Mat kMat = new Mat();
     Mat scaledTvec;
 
-    @SuppressWarnings("DuplicatedCode") // yes I know we have another solvePNP pipe
-    private Transform2d correctLocationForCameraPitch(
-            Mat tVec, Mat rVec, Rotation2d cameraPitchAngle) {
-        // Algorithm from team 5190 Green Hope Falcons. Can also be found in Ligerbot's vision
-        // whitepaper
-        var tiltAngle = cameraPitchAngle.getRadians();
-
-        // the left/right distance to the target, unchanged by tilt.
-        var x = tVec.get(0, 0)[0];
-
-        // Z distance in the flat plane is given by
-        // Z_field = z cos theta + y sin theta.
-        // Z is the distance "out" of the camera (straight forward).
-        var zField = tVec.get(2, 0)[0] * Math.cos(tiltAngle) + tVec.get(1, 0)[0] * Math.sin(tiltAngle);
-
-        Calib3d.Rodrigues(rVec, rotationMatrix);
-        Core.transpose(rotationMatrix, inverseRotationMatrix);
-
-        scaledTvec = matScale(tVec, -1);
-
-        Core.gemm(inverseRotationMatrix, scaledTvec, 1, kMat, 0, pzeroWorld);
-        scaledTvec.release();
-
-        var angle2 = Math.atan2(pzeroWorld.get(0, 0)[0], pzeroWorld.get(2, 0)[0]);
-
-        // target rotation is the rotation of the target relative to straight ahead. this number
-        // should be unchanged if the robot purely translated left/right.
-        var targetRotation = -angle2; // radians
-
-        // We want a vector that is X forward and Y left.
-        // We have a Z_field (out of the camera projected onto the field), and an X left/right.
-        // so Z_field becomes X, and X becomes Y
-
-        var targetLocation = new Translation2d(zField, -x);
-        return new Transform2d(targetLocation, new Rotation2d(targetRotation));
-    }
-
     /**
      * Element-wise scale a matrix by a given factor
      *
@@ -156,15 +127,11 @@ public class SolvePNPPipe
 
     public static class SolvePNPPipeParams {
         private final CameraCalibrationCoefficients cameraCoefficients;
-        private final Rotation2d cameraPitchAngle;
         private final TargetModel targetModel;
 
         public SolvePNPPipeParams(
-                CameraCalibrationCoefficients cameraCoefficients,
-                Rotation2d cameraPitchAngle,
-                TargetModel targetModel) {
+                CameraCalibrationCoefficients cameraCoefficients, TargetModel targetModel) {
             this.cameraCoefficients = cameraCoefficients;
-            this.cameraPitchAngle = cameraPitchAngle;
             this.targetModel = targetModel;
         }
     }

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java

@@ -0,0 +1,160 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
+import java.util.ArrayList;
+import java.util.List;
+import org.opencv.core.Mat;
+import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.raspi.PicamJNI;
+import org.photonvision.vision.apriltag.AprilTagDetectorParams;
+import org.photonvision.vision.apriltag.DetectionResult;
+import org.photonvision.vision.camera.CameraQuirk;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.opencv.CVMat;
+import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
+import org.photonvision.vision.pipe.impl.*;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TrackedTarget;
+import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
+
+@SuppressWarnings("DuplicatedCode")
+public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipelineSettings> {
+    private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
+    private final GrayscalePipe grayscalePipe = new GrayscalePipe();
+    private final AprilTagDetectionPipe aprilTagDetectionPipe = new AprilTagDetectionPipe();
+    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
+
+    public AprilTagPipeline() {
+        settings = new AprilTagPipelineSettings();
+    }
+
+    public AprilTagPipeline(AprilTagPipelineSettings settings) {
+        this.settings = settings;
+    }
+
+    @Override
+    protected void setPipeParamsImpl() {
+        // Sanitize thread count - not supported to have fewer than 1 threads
+        settings.threads = Math.max(1, settings.threads);
+
+        RotateImagePipe.RotateImageParams rotateImageParams =
+                new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
+        rotateImagePipe.setParams(rotateImageParams);
+
+        if (cameraQuirks.hasQuirk(CameraQuirk.PiCam) && PicamJNI.isSupported()) {
+            // TODO: Picam grayscale
+            PicamJNI.setRotation(settings.inputImageRotationMode.value);
+            PicamJNI.setShouldCopyColor(true); // need the color image to grayscale
+        }
+
+        AprilTagDetectorParams aprilTagDetectionParams =
+                new AprilTagDetectorParams(
+                        settings.tagFamily,
+                        settings.decimate,
+                        settings.blur,
+                        settings.threads,
+                        settings.debug,
+                        settings.refineEdges);
+
+        // TODO (HACK): tag width is Fun because it really belongs in the "target model"
+        // We need the tag width for the JNI to figure out target pose, but we need a
+        // target model for the draw 3d targets pipeline to work...
+
+        // for now, hard code tag width based on enum value
+        double tagWidth = 0.16; // guess at 200mm??
+        switch (settings.targetModel) {
+            case k200mmAprilTag:
+                {
+                    tagWidth = Units.inchesToMeters(3.25 * 2);
+                    break;
+                }
+            default:
+                {
+                    break;
+                }
+        }
+
+        aprilTagDetectionPipe.setParams(
+                new AprilTagDetectionPipeParams(
+                        aprilTagDetectionParams,
+                        frameStaticProperties.cameraCalibration,
+                        settings.numIterations,
+                        tagWidth));
+    }
+
+    @Override
+    protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings settings) {
+        long sumPipeNanosElapsed = 0L;
+
+        CVPipeResult<Mat> grayscalePipeResult;
+        Mat rawInputMat;
+        boolean inputSingleChannel = frame.image.getMat().channels() == 1;
+
+        if (inputSingleChannel) {
+            rawInputMat = new Mat(PicamJNI.grabFrame(true));
+            frame.image.getMat().release(); // release the 8bit frame ASAP.
+        } else {
+            rawInputMat = frame.image.getMat();
+            var rotateImageResult = rotateImagePipe.run(rawInputMat);
+            sumPipeNanosElapsed += rotateImageResult.nanosElapsed;
+        }
+
+        grayscalePipeResult = grayscalePipe.run(rawInputMat);
+        sumPipeNanosElapsed += grayscalePipeResult.nanosElapsed;
+
+        List<TrackedTarget> targetList;
+        CVPipeResult<List<DetectionResult>> tagDetectionPipeResult;
+
+        // Use the solvePNP Enabled flag to enable native pose estimation
+        aprilTagDetectionPipe.setNativePoseEstimationEnabled(settings.solvePNPEnabled);
+
+        tagDetectionPipeResult = aprilTagDetectionPipe.run(grayscalePipeResult.output);
+        grayscalePipeResult.output.release();
+        sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
+
+        targetList = new ArrayList<>();
+        for (DetectionResult detection : tagDetectionPipeResult.output) {
+            // populate the target list
+            // Challenge here is that TrackedTarget functions with OpenCV Contour
+            TrackedTarget target =
+                    new TrackedTarget(
+                            detection,
+                            new TargetCalculationParameters(
+                                    false, null, null, null, null, frameStaticProperties));
+
+            var correctedPose = MathUtils.convertApriltagtoPhotonPose(target.getCameraToTarget3d());
+            target.setCameraToTarget3d(
+                    new Transform3d(correctedPose.getTranslation(), correctedPose.getRotation()));
+
+            targetList.add(target);
+        }
+
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
+        var inputFrame = new Frame(new CVMat(rawInputMat), frameStaticProperties);
+        // empty output frame
+        var outputFrame =
+                Frame.emptyFrame(frameStaticProperties.imageWidth, frameStaticProperties.imageHeight);
+
+        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, outputFrame, inputFrame);
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java

@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import com.fasterxml.jackson.annotation.JsonTypeName;
+import java.util.Objects;
+import org.photonvision.vision.apriltag.AprilTagFamily;
+import org.photonvision.vision.target.TargetModel;
+
+@JsonTypeName("AprilTagPipelineSettings")
+public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
+    public AprilTagFamily tagFamily = AprilTagFamily.kTag36h11;
+    public double decimate = 1.0;
+    public double blur = 0;
+    public int threads = 1;
+    public boolean debug = false;
+    public boolean refineEdges = true;
+    public int numIterations = 200;
+
+    // 3d settings
+
+    public AprilTagPipelineSettings() {
+        super();
+        pipelineType = PipelineType.AprilTag;
+        outputShowMultipleTargets = true;
+        targetModel = TargetModel.k200mmAprilTag;
+        cameraExposure = -1;
+        ledMode = false;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        if (!super.equals(o)) return false;
+        AprilTagPipelineSettings that = (AprilTagPipelineSettings) o;
+        return Objects.equals(tagFamily, that.tagFamily)
+                && Double.compare(decimate, that.decimate) == 0
+                && Double.compare(blur, that.blur) == 0
+                && threads == that.threads
+                && debug == that.debug
+                && refineEdges == that.refineEdges;
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java

@@ -31,7 +31,8 @@ import org.photonvision.vision.opencv.ImageRotationMode;
 @JsonSubTypes({
     @JsonSubTypes.Type(value = ColoredShapePipelineSettings.class),
     @JsonSubTypes.Type(value = ReflectivePipelineSettings.class),
-    @JsonSubTypes.Type(value = DriverModePipelineSettings.class)
+    @JsonSubTypes.Type(value = DriverModePipelineSettings.class),
+    @JsonSubTypes.Type(value = AprilTagPipelineSettings.class)
 })
 public class CVPipelineSettings implements Cloneable {
     public int pipelineIndex = 0;
@@ -39,14 +40,15 @@ public class CVPipelineSettings implements Cloneable {
     public ImageFlipMode inputImageFlipMode = ImageFlipMode.NONE;
     public ImageRotationMode inputImageRotationMode = ImageRotationMode.DEG_0;
     public String pipelineNickname = "New Pipeline";
-    public double cameraExposure = 50;
+    // Only used if the pipeline type does not enable auto-exposure
+    public double cameraExposure = 100;
     public int cameraBrightness = 50;
     // Currently only used by a few cameras (notably the zero-copy Pi Camera driver) with the Gain
     // quirk
     public int cameraGain = 50;
     // Currently only used by the zero-copy Pi Camera driver
-    public int cameraRedGain = 50;
-    public int cameraBlueGain = 50;
+    public int cameraRedGain = 18;
+    public int cameraBlueGain = 24;
     public int cameraVideoModeIndex = 0;
     public FrameDivisor streamingFrameDivisor = FrameDivisor.NONE;
     public boolean ledMode = false;

photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java

@@ -162,9 +162,7 @@ public class ColoredShapePipeline
 
         var solvePNPParams =
                 new SolvePNPPipe.SolvePNPPipeParams(
-                        frameStaticProperties.cameraCalibration,
-                        frameStaticProperties.cameraPitch,
-                        settings.targetModel);
+                        frameStaticProperties.cameraCalibration, settings.targetModel);
         solvePNPPipe.setParams(solvePNPParams);
 
         Draw2dTargetsPipe.Draw2dTargetsParams draw2DTargetsParams =

photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java

@@ -37,6 +37,8 @@ public class OutputStreamPipeline {
     private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
     private final Draw2dTargetsPipe draw2dTargetsPipe = new Draw2dTargetsPipe();
     private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
+    private final Draw2dAprilTagsPipe draw2dAprilTagsPipe = new Draw2dAprilTagsPipe();
+    private final Draw3dAprilTagsPipe draw3dAprilTagsPipe = new Draw3dAprilTagsPipe();
     private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
     private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
 
@@ -58,6 +60,13 @@ public class OutputStreamPipeline {
                         settings.streamingFrameDivisor);
         draw2dTargetsPipe.setParams(draw2DTargetsParams);
 
+        var draw2DAprilTagsParams =
+                new Draw2dAprilTagsPipe.Draw2dAprilTagsParams(
+                        settings.outputShouldDraw,
+                        settings.outputShowMultipleTargets,
+                        settings.streamingFrameDivisor);
+        draw2dAprilTagsPipe.setParams(draw2DAprilTagsParams);
+
         var draw2dCrosshairParams =
                 new Draw2dCrosshairPipe.Draw2dCrosshairParams(
                         settings.outputShouldDraw,
@@ -76,6 +85,14 @@ public class OutputStreamPipeline {
                         settings.streamingFrameDivisor);
         draw3dTargetsPipe.setParams(draw3dTargetsParams);
 
+        var draw3dAprilTagsParams =
+                new Draw3dAprilTagsPipe.Draw3dAprilTagsParams(
+                        settings.outputShouldDraw,
+                        frameStaticProperties.cameraCalibration,
+                        settings.targetModel,
+                        settings.streamingFrameDivisor);
+        draw3dAprilTagsPipe.setParams(draw3dAprilTagsParams);
+
         resizeImagePipe.setParams(
                 new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
     }
@@ -96,38 +113,52 @@ public class OutputStreamPipeline {
         sumPipeNanosElapsed += pipeProfileNanos[1] = resizeImagePipe.run(outMat).nanosElapsed;
 
         // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
-        var outputMatPipeResult = outputMatPipe.run(outMat);
-        sumPipeNanosElapsed += pipeProfileNanos[2] = outputMatPipeResult.nanosElapsed;
+        if (outMat.channels() == 1) {
+            var outputMatPipeResult = outputMatPipe.run(outMat);
+            sumPipeNanosElapsed += pipeProfileNanos[2] = outputMatPipeResult.nanosElapsed;
+        } else {
+            pipeProfileNanos[2] = 0;
+        }
 
         // Draw 2D Crosshair on input and output
         var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
         sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
 
-        var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
-        sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
+        if (!(settings instanceof AprilTagPipelineSettings)) {
+            var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
+            sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
 
-        // Draw 3D Targets on input and output if necessary
-        if (settings.solvePNPEnabled
-                || (settings.solvePNPEnabled
-                        && settings instanceof ColoredShapePipelineSettings
-                        && ((ColoredShapePipelineSettings) settings).contourShape == ContourShape.Circle)) {
-            var drawOnInputResult = draw3dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
-            sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+            // Draw 3D Targets on input and output if necessary
+            if (settings.solvePNPEnabled
+                    || (settings.solvePNPEnabled
+                            && settings instanceof ColoredShapePipelineSettings
+                            && ((ColoredShapePipelineSettings) settings).contourShape == ContourShape.Circle)) {
+                var drawOnInputResult = draw3dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
+                sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+
+                var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
+                sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
+            } else {
+                pipeProfileNanos[7] = 0;
+                pipeProfileNanos[8] = 0;
+
+                var draw2dTargetsOnInput = draw2dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
+                sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+
+                var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
+                sumPipeNanosElapsed += pipeProfileNanos[6] = draw2dTargetsOnOutput.nanosElapsed;
+            }
 
-            var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
-            sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
         } else {
-            pipeProfileNanos[7] = 0;
-            pipeProfileNanos[8] = 0;
+            if (settings.solvePNPEnabled) {
+                var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(inMat, targetsToDraw));
+                sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+            } else {
+                var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(inMat, targetsToDraw));
+                sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+            }
         }
 
-        // Draw 2D contours on input and output
-        var draw2dTargetsOnInput = draw2dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
-        sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
-
-        var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
-        sumPipeNanosElapsed += pipeProfileNanos[6] = draw2dTargetsOnOutput.nanosElapsed;
-
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 

photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineType.java

@@ -22,7 +22,8 @@ public enum PipelineType {
     Calib3d(-2, Calibrate3dPipeline.class),
     DriverMode(-1, DriverModePipeline.class),
     Reflective(0, ReflectivePipeline.class),
-    ColoredShape(1, ColoredShapePipeline.class);
+    ColoredShape(1, ColoredShapePipeline.class),
+    AprilTag(2, AprilTagPipeline.class);
 
     public final int baseIndex;
     public final Class clazz;

photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java

@@ -140,9 +140,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
 
         var solvePNPParams =
                 new SolvePNPPipe.SolvePNPPipeParams(
-                        frameStaticProperties.cameraCalibration,
-                        frameStaticProperties.cameraPitch,
-                        settings.targetModel);
+                        frameStaticProperties.cameraCalibration, settings.targetModel);
         solvePNPPipe.setParams(solvePNPParams);
     }
 

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

@@ -188,6 +188,9 @@ public class PipelineManager {
                     currentUserPipeline =
                             new ColoredShapePipeline((ColoredShapePipelineSettings) desiredPipelineSettings);
                     break;
+                case AprilTag:
+                    currentUserPipeline =
+                            new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings);
             }
         }
     }
@@ -269,6 +272,12 @@ public class PipelineManager {
                     added.pipelineNickname = nickname;
                     return added;
                 }
+            case AprilTag:
+                {
+                    var added = new AprilTagPipelineSettings();
+                    added.pipelineNickname = nickname;
+                    return added;
+                }
             default:
                 {
                     logger.error("Got invalid pipeline type: " + type.toString());

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

@@ -44,6 +44,7 @@ import org.photonvision.vision.frame.consumer.FileSaveFrameConsumer;
 import org.photonvision.vision.frame.consumer.MJPGFrameConsumer;
 import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
 import org.photonvision.vision.pipeline.OutputStreamPipeline;
+import org.photonvision.vision.pipeline.PipelineType;
 import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
 import org.photonvision.vision.pipeline.UICalibrationData;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
@@ -107,7 +108,7 @@ public class VisionModule {
                         if (it.cameraGain == -1) it.cameraGain = 20; // Sane default
                     });
         }
-        if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
+        if (cameraQuirks.hasQuirk(CameraQuirk.AWBGain)) {
             pipelineManager.userPipelineSettings.forEach(
                     it -> {
                         if (it.cameraRedGain == -1) it.cameraRedGain = 16; // Sane defaults
@@ -273,7 +274,9 @@ public class VisionModule {
                 if (shouldRun) {
                     consumeRawResults(inputFrame, outputFrame, targets);
                     try {
-                        var osr = outputStreamPipeline.process(inputFrame, outputFrame, settings, targets);
+                        CVPipelineResult osr =
+                                outputStreamPipeline.process(inputFrame, outputFrame, settings, targets);
+
                         consumeFpsLimitedResult(osr);
                     } catch (Exception e) {
                         // Never die
@@ -297,12 +300,6 @@ public class VisionModule {
         }
     }
 
-    void setDriverMode(boolean isDriverMode) {
-        pipelineManager.setDriverMode(isDriverMode);
-        setVisionLEDs(!isDriverMode);
-        saveAndBroadcastAll();
-    }
-
     public void start() {
         visionRunner.startProcess();
         streamRunnable.start();
@@ -310,16 +307,7 @@ public class VisionModule {
 
     public void setFovAndPitch(double fov, Rotation2d pitch) {
         var settables = visionSource.getSettables();
-        logger.trace(
-                () ->
-                        "Setting "
-                                + settables.getConfiguration().nickname
-                                + ": pitch ("
-                                + pitch.getDegrees()
-                                + ") FOV ("
-                                + fov
-                                + ")");
-        settables.setCameraPitch(pitch);
+        logger.trace(() -> "Setting " + settables.getConfiguration().nickname + ") FOV (" + fov + ")");
 
         // Only set FOV if we have no vendor JSON and we aren't using a PiCAM
         if (isVendorCamera()) {
@@ -333,6 +321,22 @@ public class VisionModule {
         return visionSource.isVendorCamera();
     }
 
+    void changePipelineType(int newType) {
+        pipelineManager.changePipelineType(newType);
+        setPipeline(pipelineManager.getCurrentPipelineIndex());
+        saveAndBroadcastAll();
+    }
+
+    void setDriverMode(boolean isDriverMode) {
+        pipelineManager.setDriverMode(isDriverMode);
+        setVisionLEDs(!isDriverMode);
+        setPipeline(
+                isDriverMode
+                        ? PipelineManager.DRIVERMODE_INDEX
+                        : pipelineManager.getCurrentPipelineIndex());
+        saveAndBroadcastAll();
+    }
+
     public void startCalibration(UICalibrationData data) {
         var settings = pipelineManager.calibration3dPipeline.getSettings();
         pipelineManager.calibration3dPipeline.deleteSavedImages();
@@ -352,11 +356,13 @@ public class VisionModule {
         if (!cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
             settings.cameraGain = -1;
         }
-        if (!cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
+        if (!cameraQuirks.hasQuirk(CameraQuirk.AWBGain)) {
             settings.cameraRedGain = -1;
             settings.cameraBlueGain = -1;
         }
 
+        settings.cameraExposure = -1;
+
         setPipeline(PipelineManager.CAL_3D_INDEX);
     }
 
@@ -383,37 +389,54 @@ public class VisionModule {
     void setPipeline(int index) {
         logger.info("Setting pipeline to " + index);
         pipelineManager.setIndex(index);
-        var config = pipelineManager.getPipelineSettings(index);
+        var pipelineSettings = pipelineManager.getPipelineSettings(index);
 
-        if (config == null) {
+        if (pipelineSettings == null) {
             logger.error("Config for index " + index + " was null!");
             return;
         }
 
-        visionSource.getSettables().setVideoModeInternal(config.cameraVideoModeIndex);
-        visionSource.getSettables().setBrightness(config.cameraBrightness);
-        visionSource.getSettables().setExposure(config.cameraExposure);
-        visionSource.getSettables().setGain(config.cameraGain);
+        visionSource.getSettables().setVideoModeInternal(pipelineSettings.cameraVideoModeIndex);
+        visionSource.getSettables().setBrightness(pipelineSettings.cameraBrightness);
+        visionSource.getSettables().setGain(pipelineSettings.cameraGain);
+
+        // set to true to change camera gain/exposure settings for low exposure
+        // false will keep the camera running in a more "nice-for-humans" mode
+        // Each camera type is allowed to decide what settings that exactly means
+        boolean lowExposureOptimization =
+                (pipelineSettings.pipelineType == PipelineType.ColoredShape
+                        || pipelineSettings.pipelineType == PipelineType.Reflective);
+        visionSource.getSettables().setLowExposureOptimization(lowExposureOptimization);
+
+        if (lowExposureOptimization) {
+            if (pipelineSettings.cameraExposure == -1)
+                pipelineSettings.cameraExposure = 10; // reasonable default
+        } else {
+            // in human-friendly mode, exposure is automatic
+            pipelineSettings.cameraExposure = -1;
+        }
+        visionSource.getSettables().setExposure(pipelineSettings.cameraExposure);
 
         if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
             // If the gain is disabled for some reason, re-enable it
-            if (config.cameraGain == -1) config.cameraGain = 20;
-            visionSource.getSettables().setGain(Math.max(0, config.cameraGain));
+            if (pipelineSettings.cameraGain == -1) pipelineSettings.cameraGain = 20;
+            visionSource.getSettables().setGain(Math.max(0, pipelineSettings.cameraGain));
         } else {
-            config.cameraGain = -1;
-        }
-        if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
-            // If the AWB gains are disabled for some reason, re-enable it
-            if (config.cameraRedGain == -1) config.cameraRedGain = 16;
-            if (config.cameraBlueGain == -1) config.cameraBlueGain = 16;
-            visionSource.getSettables().setRedGain(Math.max(0, config.cameraRedGain));
-            visionSource.getSettables().setBlueGain(Math.max(0, config.cameraBlueGain));
-        } else {
-            config.cameraRedGain = -1;
-            config.cameraBlueGain = -1;
+            pipelineSettings.cameraGain = -1;
         }
 
-        setVisionLEDs(config.ledMode);
+        if (cameraQuirks.hasQuirk(CameraQuirk.AWBGain)) {
+            // If the AWB gains are disabled for some reason, re-enable it
+            if (pipelineSettings.cameraRedGain == -1) pipelineSettings.cameraRedGain = 16;
+            if (pipelineSettings.cameraBlueGain == -1) pipelineSettings.cameraBlueGain = 16;
+            visionSource.getSettables().setRedGain(Math.max(0, pipelineSettings.cameraRedGain));
+            visionSource.getSettables().setBlueGain(Math.max(0, pipelineSettings.cameraBlueGain));
+        } else {
+            pipelineSettings.cameraRedGain = -1;
+            pipelineSettings.cameraBlueGain = -1;
+        }
+
+        setVisionLEDs(pipelineSettings.ledMode);
 
         visionSource.getSettables().getConfiguration().currentPipelineIndex =
                 pipelineManager.getCurrentPipelineIndex();
@@ -477,7 +500,6 @@ public class VisionModule {
         var ret = new PhotonConfiguration.UICameraConfiguration();
 
         ret.fov = visionSource.getSettables().getFOV();
-        ret.tiltDegrees = this.visionSource.getSettables().getCameraPitch().getDegrees();
         ret.nickname = visionSource.getSettables().getConfiguration().nickname;
         ret.currentPipelineSettings =
                 SerializationUtils.objectToHashMap(pipelineManager.getCurrentPipelineSettings());

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

@@ -156,7 +156,7 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
                         }
                         return;
                     case "changePipelineType":
-                        parentModule.pipelineManager.changePipelineType((Integer) newPropValue);
+                        parentModule.changePipelineType((Integer) newPropValue);
                         parentModule.saveAndBroadcastAll();
                         return;
                 }

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

@@ -31,6 +31,7 @@ import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.raspi.PicamJNI;
+import org.photonvision.vision.camera.CameraQuirk;
 import org.photonvision.vision.camera.CameraType;
 import org.photonvision.vision.camera.USBCameraSource;
 import org.photonvision.vision.camera.ZeroCopyPicamSource;
@@ -314,7 +315,12 @@ public class VisionSourceManager {
                 cameraSources.add(piCamSrc);
                 continue;
             }
-            cameraSources.add(new USBCameraSource(configuration));
+
+            var newCam = new USBCameraSource(configuration);
+
+            if (!newCam.cameraQuirks.hasQuirk(CameraQuirk.CompletelyBroken)) {
+                cameraSources.add(newCam);
+            }
         }
         return cameraSources;
     }

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

@@ -18,7 +18,6 @@
 package org.photonvision.vision.processes;
 
 import edu.wpi.first.cscore.VideoMode;
-import edu.wpi.first.math.geometry.Rotation2d;
 import java.util.HashMap;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.logging.LogGroup;
@@ -65,17 +64,10 @@ public abstract class VisionSourceSettables {
         calculateFrameStaticProps();
     }
 
+    public abstract void setLowExposureOptimization(boolean mode);
+
     protected abstract void setVideoModeInternal(VideoMode videoMode);
 
-    public void setCameraPitch(Rotation2d pitch) {
-        configuration.camPitch = pitch;
-        calculateFrameStaticProps();
-    }
-
-    public Rotation2d getCameraPitch() {
-        return configuration.camPitch;
-    }
-
     @SuppressWarnings("unused")
     public void setVideoModeIndex(int index) {
         setVideoMode(videoModes.get(index));
@@ -103,7 +95,6 @@ public abstract class VisionSourceSettables {
                 new FrameStaticProperties(
                         videoMode,
                         getFOV(),
-                        configuration.camPitch,
                         configuration.calibrations.stream()
                                 .filter(
                                         it ->

photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java

@@ -47,6 +47,7 @@ public enum TargetModel implements Releasable {
                             Units.inchesToMeters(2d * 12d + 5.25)),
                     new Point3(Units.inchesToMeters(19.625), 0, Units.inchesToMeters(2d * 12d + 5.25))),
             Units.inchesToMeters(12)),
+
     k2019DualTarget(
             List.of(
                     new Point3(Units.inchesToMeters(-5.936), Units.inchesToMeters(2.662), 0),
@@ -54,6 +55,7 @@ public enum TargetModel implements Releasable {
                     new Point3(Units.inchesToMeters(7.313), Units.inchesToMeters(-2.662), 0),
                     new Point3(Units.inchesToMeters(5.936), Units.inchesToMeters(2.662), 0)),
             0.1),
+
     kCircularPowerCell7in(
             List.of(
                     new Point3(
@@ -99,7 +101,14 @@ public enum TargetModel implements Releasable {
                     new Point3(Units.inchesToMeters(10), Units.inchesToMeters(0), 0),
                     new Point3(Units.inchesToMeters(10), Units.inchesToMeters(12), 0)),
             Units.inchesToMeters(6)),
-    ;
+    k200mmAprilTag( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
+            // do not
+            List.of(
+                    new Point3(-Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0),
+                    new Point3(Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0),
+                    new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0),
+                    new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)),
+            -Units.inchesToMeters(3.25 * 2));
 
     @JsonIgnore private MatOfPoint3f realWorldTargetCoordinates;
     @JsonIgnore private MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f();

photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java

@@ -16,13 +16,17 @@
  */
 package org.photonvision.vision.target;
 
-import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import java.util.HashMap;
 import java.util.List;
+import org.opencv.core.CvType;
 import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
 import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
 import org.opencv.core.RotatedRect;
+import org.photonvision.vision.apriltag.DetectionResult;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.*;
 
@@ -42,10 +46,12 @@ public class TrackedTarget implements Releasable {
     private double m_area;
     private double m_skew;
 
-    private Transform2d m_cameraToTarget = new Transform2d();
+    private Transform3d m_cameraToTarget3d = new Transform3d();
 
     private CVShape m_shape;
 
+    private int m_fiducialId = -1;
+
     private Mat m_cameraRelativeTvec, m_cameraRelativeRvec;
 
     public TrackedTarget(
@@ -56,6 +62,71 @@ public class TrackedTarget implements Releasable {
         calculateValues(params);
     }
 
+    public TrackedTarget(DetectionResult result, TargetCalculationParameters params) {
+        m_targetOffsetPoint = new Point(result.getCenterX(), result.getCenterY());
+        m_robotOffsetPoint = new Point();
+
+        m_pitch =
+                TargetCalculations.calculatePitch(
+                        result.getCenterY(), params.cameraCenterPoint.y, params.verticalFocalLength);
+        m_yaw =
+                TargetCalculations.calculateYaw(
+                        result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
+        Pose3d bestPose = new Pose3d();
+        if (result.getError1() <= result.getError2()) {
+            bestPose = result.getPoseResult1();
+        } else {
+            bestPose = result.getPoseResult2();
+        }
+
+        m_cameraToTarget3d = new Transform3d(new Pose3d(), bestPose);
+
+        double[] corners = result.getCorners();
+        Point[] cornerPoints =
+                new Point[] {
+                    new Point(corners[0], corners[1]),
+                    new Point(corners[2], corners[3]),
+                    new Point(corners[4], corners[5]),
+                    new Point(corners[6], corners[7])
+                };
+        m_targetCorners = List.of(cornerPoints);
+        MatOfPoint contourMat = new MatOfPoint(cornerPoints);
+        m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints);
+        m_mainContour = new Contour(contourMat);
+        m_area = m_mainContour.getArea() / params.imageArea * 100;
+        m_fiducialId = result.getId();
+        m_shape = null;
+
+        // TODO implement skew? or just yeet
+        m_skew = 0;
+
+        var tvec = new Mat(3, 1, CvType.CV_64FC1);
+        tvec.put(
+                0,
+                0,
+                new double[] {
+                    bestPose.getTranslation().getX(),
+                    bestPose.getTranslation().getY(),
+                    bestPose.getTranslation().getZ()
+                });
+        setCameraRelativeTvec(tvec);
+
+        // Opencv expects a 3d vector with norm = angle and direction = axis
+        var rvec = new Mat(3, 1, CvType.CV_64FC1);
+        var angle = bestPose.getRotation().getAngle();
+        var axis = bestPose.getRotation().getAxis().times(angle);
+        rvec.put(0, 0, axis.getData());
+        setCameraRelativeRvec(rvec);
+    }
+
+    public void setFiducialId(int id) {
+        m_fiducialId = id;
+    }
+
+    public int getFiducialId() {
+        return m_fiducialId;
+    }
+
     /**
      * Set the approximate bouding polygon.
      *
@@ -145,12 +216,12 @@ public class TrackedTarget implements Releasable {
         return !m_subContours.isEmpty();
     }
 
-    public Transform2d getCameraToTarget() {
-        return m_cameraToTarget;
+    public Transform3d getCameraToTarget3d() {
+        return m_cameraToTarget3d;
     }
 
-    public void setCameraToTarget(Transform2d pose) {
-        this.m_cameraToTarget = pose;
+    public void setCameraToTarget3d(Transform3d pose) {
+        this.m_cameraToTarget3d = pose;
     }
 
     public Mat getCameraRelativeTvec() {
@@ -185,20 +256,31 @@ public class TrackedTarget implements Releasable {
         ret.put("yaw", getYaw());
         ret.put("skew", getSkew());
         ret.put("area", getArea());
-        if (getCameraToTarget() != null) {
-            ret.put("pose", transformToMap(getCameraToTarget()));
+        if (getCameraToTarget3d() != null) {
+            ret.put("pose", transformToMap(getCameraToTarget3d()));
         }
+        ret.put("fiducialId", getFiducialId());
         return ret;
     }
 
-    private static HashMap<String, Object> transformToMap(Transform2d transform) {
+    private static HashMap<String, Object> transformToMap(Transform3d transform) {
         var ret = new HashMap<String, Object>();
         ret.put("x", transform.getTranslation().getX());
         ret.put("y", transform.getTranslation().getY());
-        ret.put("rot", transform.getRotation().getDegrees());
+        ret.put("z", transform.getTranslation().getZ());
+        ret.put("qw", transform.getRotation().getQuaternion().getW());
+        ret.put("qx", transform.getRotation().getQuaternion().getX());
+        ret.put("qy", transform.getRotation().getQuaternion().getY());
+        ret.put("qz", transform.getRotation().getQuaternion().getZ());
+
+        ret.put("angle_z", transform.getRotation().getZ() + Math.PI / 2.0);
         return ret;
     }
 
+    public boolean isFiducial() {
+        return this.m_fiducialId >= 0;
+    }
+
     public static class TargetCalculationParameters {
         // TargetOffset calculation values
         final boolean isLandscape;

photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java

@@ -19,7 +19,6 @@ package org.photonvision.vision.pipeline;
 
 import static org.junit.jupiter.api.Assertions.*;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import java.io.File;
 import java.nio.file.Path;
@@ -104,7 +103,7 @@ public class Calibrate3dPipeTest {
             var frame =
                     new Frame(
                             new CVMat(Imgcodecs.imread(file.getAbsolutePath())),
-                            new FrameStaticProperties(640, 480, 60, new Rotation2d(), null));
+                            new FrameStaticProperties(640, 480, 60, null));
             var output = calibration3dPipeline.run(frame, QuirkyCamera.DefaultCamera);
             // TestUtils.showImage(output.outputFrame.image.getMat());
             output.release();
@@ -120,7 +119,7 @@ public class Calibrate3dPipeTest {
         var frame =
                 new Frame(
                         new CVMat(Imgcodecs.imread(directoryListing[0].getAbsolutePath())),
-                        new FrameStaticProperties(640, 480, 60, new Rotation2d(), null));
+                        new FrameStaticProperties(640, 480, 60, null));
         calibration3dPipeline.run(frame, QuirkyCamera.DefaultCamera).release();
         frame.release();
 
@@ -267,8 +266,7 @@ public class Calibrate3dPipeTest {
                 var frame =
                         new Frame(
                                 new CVMat(Imgcodecs.imread(file.getAbsolutePath())),
-                                new FrameStaticProperties(
-                                        (int) imgRes.width, (int) imgRes.height, 67, new Rotation2d(), null));
+                                new FrameStaticProperties((int) imgRes.width, (int) imgRes.height, 67, null));
                 var output = calibration3dPipeline.run(frame, QuirkyCamera.DefaultCamera);
 
                 // TestUtils.showImage(output.outputFrame.image.getMat(), file.getName(), 1);

photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java

@@ -19,7 +19,6 @@ package org.photonvision.vision.pipeline;
 
 import static org.junit.jupiter.api.Assertions.*;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import java.util.stream.Collectors;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
@@ -111,7 +110,6 @@ public class CirclePNPTest {
                 new FileFrameProvider(
                         TestUtils.getPowercellImagePath(TestUtils.PowercellTestImages.kPowercell_test_6, false),
                         TestUtils.WPI2020Image.FOV,
-                        new Rotation2d(),
                         TestUtils.get2020LifeCamCoeffs(true));
 
         CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
@@ -163,7 +161,7 @@ public class CirclePNPTest {
         System.out.println(
                 "Found targets at "
                         + pipelineResult.targets.stream()
-                                .map(TrackedTarget::getCameraToTarget)
+                                .map(TrackedTarget::getCameraToTarget3d)
                                 .collect(Collectors.toList()));
     }
 }

photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java

@@ -20,13 +20,12 @@ package org.photonvision.vision.pipeline;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
 
-import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import java.util.stream.Collectors;
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
-import org.opencv.imgcodecs.Imgcodecs;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.camera.QuirkyCamera;
@@ -104,7 +103,6 @@ public class SolvePNPTest {
                 new FileFrameProvider(
                         TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark48in, false),
                         TestUtils.WPI2019Image.FOV,
-                        new Rotation2d(),
                         TestUtils.get2019LifeCamCoeffs(false));
 
         CVPipelineResult pipelineResult;
@@ -113,12 +111,20 @@ public class SolvePNPTest {
         printTestResults(pipelineResult);
 
         // these numbers are not *accurate*, but they are known and expected
-        var pose = pipelineResult.targets.get(0).getCameraToTarget();
+        var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
         Assertions.assertEquals(1.1, pose.getTranslation().getX(), 0.05);
         Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.05);
-        Assertions.assertEquals(1, pose.getRotation().getDegrees(), 1);
 
-        Imgcodecs.imwrite("D:\\out.jpg", pipelineResult.outputFrame.image.getMat());
+        // We expect the object X axis to be to the right, or negative-Y in world space
+        Assertions.assertEquals(
+                -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY(), 0.05);
+        // We expect the object Y axis to be up, or +Z in world space
+        Assertions.assertEquals(
+                1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ(), 0.05);
+        // We expect the object Z axis to towards the camera, or negative-X in world space
+        Assertions.assertEquals(
+                -1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX(), 0.05);
+
         TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999);
     }
 
@@ -139,19 +145,18 @@ public class SolvePNPTest {
                 new FileFrameProvider(
                         TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_224in_Left, false),
                         TestUtils.WPI2020Image.FOV,
-                        new Rotation2d(),
                         TestUtils.get2020LifeCamCoeffs(false));
 
         CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
         printTestResults(pipelineResult);
 
         // these numbers are not *accurate*, but they are known and expected
-        var pose = pipelineResult.targets.get(0).getCameraToTarget();
+        var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
         Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05);
         Assertions.assertEquals(Units.inchesToMeters(35), pose.getTranslation().getY(), 0.05);
-        Assertions.assertEquals(42, pose.getRotation().getDegrees(), 1);
+        Assertions.assertEquals(Units.degreesToRadians(-42), pose.getRotation().getZ(), 1);
 
-        TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999);
+        TestUtils.showImage(pipelineResult.inputFrame.image.getMat(), "Pipeline output", 999999);
     }
 
     private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) {
@@ -197,7 +202,7 @@ public class SolvePNPTest {
         System.out.println(
                 "Found targets at "
                         + pipelineResult.targets.stream()
-                                .map(TrackedTarget::getCameraToTarget)
+                                .map(TrackedTarget::getCameraToTarget3d)
                                 .collect(Collectors.toList()));
     }
 }

photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java

@@ -20,7 +20,6 @@ package org.photonvision.vision.processes;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.cscore.VideoMode;
-import edu.wpi.first.math.geometry.Rotation2d;
 import java.util.Arrays;
 import java.util.HashMap;
 import java.util.List;
@@ -79,6 +78,9 @@ public class VisionModuleManagerTest {
         @Override
         public void setGain(int gain) {}
 
+        @Override
+        public void setLowExposureOptimization(boolean mode) {}
+
         @Override
         public VideoMode getCurrentVideoMode() {
             return new VideoMode(0, 320, 240, 254);
@@ -86,8 +88,7 @@ public class VisionModuleManagerTest {
 
         @Override
         public void setVideoModeInternal(VideoMode videoMode) {
-            this.frameStaticProperties =
-                    new FrameStaticProperties(getCurrentVideoMode(), getFOV(), new Rotation2d(), null);
+            this.frameStaticProperties = new FrameStaticProperties(getCurrentVideoMode(), getFOV(), null);
         }
 
         @Override

photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java

@@ -18,7 +18,6 @@ package org.photonvision.vision.target;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
@@ -37,8 +36,7 @@ public class TargetCalculationsTest {
     private static final double diagFOV = Math.toRadians(70.0);
 
     private static final FrameStaticProperties props =
-            new FrameStaticProperties(
-                    (int) imageSize.width, (int) imageSize.height, diagFOV, new Rotation2d(), null);
+            new FrameStaticProperties((int) imageSize.width, (int) imageSize.height, diagFOV, null);
     private static final TrackedTarget.TargetCalculationParameters params =
             new TrackedTarget.TargetCalculationParameters(
                     true,