Aruco Support for AprilTag Detection (Experimental) (#636)

Uses OpenCV's aruco module for AprilTag detection.

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

@@ -197,6 +197,7 @@ public class TestUtils {
     public enum ApriltagTestImages {
         kRobots,
         kTag1_640_480,
+        kTag1_16h5_1280,
         kTag_corner_1280;
 
         public final Path path;
@@ -204,7 +205,9 @@ public class TestUtils {
         Path getPath() {
             // Strip leading k
             var filename = this.toString().substring(1).toLowerCase();
-            return Path.of("apriltag", filename + ".jpg");
+            var extension = ".jpg";
+            if (filename.equals("tag1_16h5_1280")) extension = ".png";
+            return Path.of("apriltag", filename + extension);
         }
 
         ApriltagTestImages() {

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

@@ -172,6 +172,17 @@ public class MathUtils {
     private static final Rotation3d WPILIB_BASE_ROTATION =
             new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 1, 0, 0, 0, 1, 1, 0, 0));
 
+    public static Transform3d convertOpenCVtoPhotonTransform(Transform3d cameraToTarget3d) {
+        // TODO: Refactor into new pipe?
+        // CameraToTarget _should_ be in opencv-land EDN
+        var nwu =
+                CoordinateSystem.convert(
+                        new Pose3d().transformBy(cameraToTarget3d),
+                        CoordinateSystem.EDN(),
+                        CoordinateSystem.NWU());
+        return new Transform3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
+    }
+
     public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
         // TODO: Refactor into new pipe?
         // CameraToTarget _should_ be in opencv-land EDN
@@ -208,6 +219,14 @@ public class MathUtils {
         return new Transform3d(pose.getTranslation(), ocvRotation);
     }
 
+    public static Pose3d convertArucotoOpenCV(Transform3d pose) {
+        var ocvRotation =
+                APRILTAG_BASE_ROTATION.rotateBy(
+                        new Rotation3d(VecBuilder.fill(0, 0, 1), Units.degreesToRadians(180))
+                                .rotateBy(pose.getRotation()));
+        return new Pose3d(pose.getTranslation(), ocvRotation);
+    }
+
     public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) {
         var angle = rotation.getAngle();
         var axis = rotation.getAxis().times(angle);

photon-core/src/main/java/org/photonvision/vision/aruco/ArucoDetectionResult.java

@@ -0,0 +1,83 @@
+/*
+ * 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.aruco;
+
+import java.util.Arrays;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class ArucoDetectionResult {
+    private static final Logger logger =
+            new Logger(ArucoDetectionResult.class, LogGroup.VisionModule);
+    double[] xCorners;
+    double[] yCorners;
+
+    int id;
+
+    double[] tvec, rvec;
+
+    public ArucoDetectionResult(
+            double[] xCorners, double[] yCorners, int id, double[] tvec, double[] rvec) {
+        this.xCorners = xCorners;
+        this.yCorners = yCorners;
+        this.id = id;
+        this.tvec = tvec;
+        this.rvec = rvec;
+        // logger.debug("Creating a new detection result: " + this.toString());
+    }
+
+    public double[] getTvec() {
+        return tvec;
+    }
+
+    public double[] getRvec() {
+        return rvec;
+    }
+
+    public double[] getxCorners() {
+        return xCorners;
+    }
+
+    public double[] getyCorners() {
+        return yCorners;
+    }
+
+    public int getId() {
+        return id;
+    }
+
+    public double getCenterX() {
+        return (xCorners[0] + xCorners[1] + xCorners[2] + xCorners[3]) * .25;
+    }
+
+    public double getCenterY() {
+        return (yCorners[0] + yCorners[1] + yCorners[2] + yCorners[3]) * .25;
+    }
+
+    @Override
+    public String toString() {
+        return "ArucoDetectionResult{"
+                + "xCorners="
+                + Arrays.toString(xCorners)
+                + ", yCorners="
+                + Arrays.toString(yCorners)
+                + ", id="
+                + id
+                + '}';
+    }
+}

photon-core/src/main/java/org/photonvision/vision/aruco/ArucoDetectorParams.java

@@ -0,0 +1,76 @@
+/*
+ * 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.aruco;
+
+import org.opencv.aruco.Aruco;
+import org.opencv.aruco.ArucoDetector;
+import org.opencv.aruco.DetectorParameters;
+import org.opencv.aruco.Dictionary;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class ArucoDetectorParams {
+    private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
+
+    private float m_decimate = -1;
+    private int m_iterations = -1;
+    private double m_accuracy = -1;
+
+    DetectorParameters parameters = DetectorParameters.create();
+    ArucoDetector detector;
+
+    public ArucoDetectorParams() {
+        setDecimation(1);
+        setCornerAccuracy(25);
+        setCornerRefinementMaxIterations(100);
+
+        detector = new ArucoDetector(Dictionary.get(Aruco.DICT_APRILTAG_16h5), parameters);
+    }
+
+    public void setDecimation(float decimate) {
+        if (decimate == m_decimate) return;
+
+        logger.info("Setting decimation from " + m_decimate + " to " + decimate);
+
+        // We only need to mutate the parameters -- the detector keeps a poitner to the parameters
+        // object internally, so it should automatically update
+        parameters.set_aprilTagQuadDecimate((float) decimate);
+        m_decimate = decimate;
+    }
+
+    public void setCornerRefinementMaxIterations(int iters) {
+        if (iters == m_iterations || iters <= 0) return;
+
+        parameters.set_cornerRefinementMethod(Aruco.CORNER_REFINE_SUBPIX);
+        parameters.set_cornerRefinementMaxIterations(iters); // 200
+
+        m_iterations = iters;
+    }
+
+    public void setCornerAccuracy(double accuracy) {
+        if (accuracy == m_accuracy || accuracy <= 0) return;
+
+        parameters.set_cornerRefinementMinAccuracy(
+                accuracy / 1000.0); // divides by 1000 because the UI multiplies it by 1000
+        m_accuracy = accuracy;
+    }
+
+    public ArucoDetector getDetector() {
+        return detector;
+    }
+}

photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java

@@ -0,0 +1,132 @@
+/*
+ * 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.aruco;
+
+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.Translation3d;
+import edu.wpi.first.math.util.Units;
+import java.util.ArrayList;
+import org.opencv.aruco.Aruco;
+import org.opencv.aruco.ArucoDetector;
+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 PhotonArucoDetector {
+    private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
+
+    private static final Rotation3d ARUCO_BASE_ROTATION =
+            new Rotation3d(VecBuilder.fill(0, 0, 1), Units.degreesToRadians(180));
+
+    Mat ids;
+
+    Mat tvecs;
+    Mat rvecs;
+    ArrayList<Mat> corners;
+
+    Mat cornerMat;
+    Translation3d translation;
+    Rotation3d rotation;
+    double timeStartDetect;
+    double timeEndDetect;
+    Pose3d tagPose;
+    double timeStartProcess;
+    double timeEndProcess;
+    double[] xCorners = new double[4];
+    double[] yCorners = new double[4];
+
+    public PhotonArucoDetector() {
+        logger.debug("New Aruco Detector");
+        ids = new Mat();
+        tvecs = new Mat();
+        rvecs = new Mat();
+        corners = new ArrayList<Mat>();
+        tagPose = new Pose3d();
+        translation = new Translation3d();
+        rotation = new Rotation3d();
+    }
+
+    public ArucoDetectionResult[] detect(
+            Mat grayscaleImg,
+            float tagSize,
+            CameraCalibrationCoefficients coeffs,
+            ArucoDetector detector) {
+        detector.detectMarkers(grayscaleImg, corners, ids);
+        if (coeffs != null) {
+            Aruco.estimatePoseSingleMarkers(
+                    corners,
+                    tagSize,
+                    coeffs.getCameraIntrinsicsMat(),
+                    coeffs.getDistCoeffsMat(),
+                    rvecs,
+                    tvecs);
+        }
+
+        ArucoDetectionResult[] toReturn = new ArucoDetectionResult[corners.size()];
+        timeStartProcess = System.currentTimeMillis();
+        for (int i = 0; i < corners.size(); i++) {
+            cornerMat = corners.get(i);
+            // logger.debug(cornerMat.dump());
+            xCorners =
+                    new double[] {
+                        cornerMat.get(0, 0)[0],
+                        cornerMat.get(0, 1)[0],
+                        cornerMat.get(0, 2)[0],
+                        cornerMat.get(0, 3)[0]
+                    };
+            yCorners =
+                    new double[] {
+                        cornerMat.get(0, 0)[1],
+                        cornerMat.get(0, 1)[1],
+                        cornerMat.get(0, 2)[1],
+                        cornerMat.get(0, 3)[1]
+                    };
+            cornerMat.release();
+
+            double[] tvec;
+            double[] rvec;
+            if (coeffs != null) {
+                // Need to apply a 180 rotation about Z
+                var origRvec = rvecs.get(i, 0);
+                var axisangle = VecBuilder.fill(origRvec[0], origRvec[1], origRvec[2]);
+                Rotation3d rotation = new Rotation3d(axisangle, axisangle.normF());
+                var ocvRotation = ARUCO_BASE_ROTATION.rotateBy(rotation);
+
+                var angle = ocvRotation.getAngle();
+                var finalAxisAngle = ocvRotation.getAxis().times(angle);
+
+                tvec = tvecs.get(i, 0);
+                rvec = finalAxisAngle.getData();
+            } else {
+                tvec = new double[] {0, 0, 0};
+                rvec = new double[] {0, 0, 0};
+            }
+
+            toReturn[i] =
+                    new ArucoDetectionResult(xCorners, yCorners, (int) ids.get(i, 0)[0], tvec, rvec);
+        }
+        rvecs.release();
+        tvecs.release();
+        ids.release();
+
+        return toReturn;
+    }
+}

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

@@ -0,0 +1,50 @@
+/*
+ * 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 edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.opencv.aruco.DetectorParameters;
+import org.opencv.core.Mat;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
+import org.photonvision.vision.aruco.PhotonArucoDetector;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class ArucoDetectionPipe
+        extends CVPipe<Mat, List<ArucoDetectionResult>, ArucoDetectionPipeParams> {
+    PhotonArucoDetector detector = new PhotonArucoDetector();
+
+    @Override
+    protected List<ArucoDetectionResult> process(Mat in) {
+        return List.of(
+                detector.detect(
+                        in,
+                        (float) Units.inchesToMeters(6),
+                        params.cameraCalibrationCoefficients,
+                        params.detectorParams));
+    }
+
+    @Override
+    public void setParams(ArucoDetectionPipeParams params) {
+        super.setParams(params);
+    }
+
+    public DetectorParameters getParameters() {
+        return params == null ? null : params.detectorParams.get_params();
+    }
+}

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

@@ -0,0 +1,57 @@
+/*
+ * 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.opencv.aruco.ArucoDetector;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+
+public class ArucoDetectionPipeParams {
+    public ArucoDetector detectorParams;
+    public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
+
+    public ArucoDetectionPipeParams(
+            ArucoDetector detector, CameraCalibrationCoefficients cameraCalibrationCoefficients) {
+        this.detectorParams = detector;
+        this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        ArucoDetectionPipeParams that = (ArucoDetectionPipeParams) o;
+        return Objects.equals(detectorParams, that.detectorParams)
+                && Objects.equals(cameraCalibrationCoefficients, that.cameraCalibrationCoefficients);
+    }
+
+    @Override
+    public int hashCode() {
+        return Objects.hash(detectorParams, cameraCalibrationCoefficients);
+    }
+
+    @Override
+    public String toString() {
+        return "ArucoDetectionPipeParams{"
+                + "detectorParams="
+                + detectorParams
+                + ", cameraCalibrationCoefficients="
+                + cameraCalibrationCoefficients
+                + '}';
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dArucoPipe.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 Draw2dArucoPipe extends Draw2dTargetsPipe {
+    public static class Draw2dArucoParams extends Draw2dTargetsPipe.Draw2dTargetsParams {
+        public Draw2dArucoParams(
+                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/Draw3dArucoPipe.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 Draw3dArucoPipe extends Draw3dTargetsPipe {
+    public static class Draw3dArucoParams extends Draw3dContoursParams {
+        public Draw3dArucoParams(
+                boolean shouldDraw,
+                CameraCalibrationCoefficients cameraCalibrationCoefficients,
+                TargetModel targetModel,
+                FrameDivisor divisor) {
+            super(shouldDraw, cameraCalibrationCoefficients, targetModel, divisor);
+            this.shouldDrawHull = false;
+        }
+    }
+}

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

@@ -0,0 +1,128 @@
+/*
+ * 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/>.
+ */
+
+/*
+ * 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 java.util.ArrayList;
+import java.util.List;
+import org.opencv.core.Mat;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
+import org.photonvision.vision.aruco.ArucoDetectorParams;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.frame.FrameThresholdType;
+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 ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSettings> {
+    private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
+    private final GrayscalePipe grayscalePipe = new GrayscalePipe();
+
+    private final ArucoDetectionPipe arucoDetectionPipe = new ArucoDetectionPipe();
+    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
+
+    ArucoDetectorParams m_arucoDetectorParams = new ArucoDetectorParams();
+
+    public ArucoPipeline() {
+        super(FrameThresholdType.GREYSCALE);
+        settings = new ArucoPipelineSettings();
+    }
+
+    public ArucoPipeline(ArucoPipelineSettings settings) {
+        super(FrameThresholdType.GREYSCALE);
+        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);
+
+        m_arucoDetectorParams.setDecimation((float) settings.decimate);
+        m_arucoDetectorParams.setCornerRefinementMaxIterations(settings.numIterations);
+        m_arucoDetectorParams.setCornerAccuracy(settings.cornerAccuracy);
+
+        arucoDetectionPipe.setParams(
+                new ArucoDetectionPipeParams(
+                        m_arucoDetectorParams.getDetector(), frameStaticProperties.cameraCalibration));
+    }
+
+    @Override
+    protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) {
+        long sumPipeNanosElapsed = 0L;
+        Mat rawInputMat;
+        rawInputMat = frame.colorImage.getMat();
+
+        List<TrackedTarget> targetList;
+        CVPipeResult<List<ArucoDetectionResult>> tagDetectionPipeResult;
+
+        if (rawInputMat.empty()) {
+            return new CVPipelineResult(sumPipeNanosElapsed, 0, List.of(), frame);
+        }
+
+        tagDetectionPipeResult = arucoDetectionPipe.run(rawInputMat);
+        targetList = new ArrayList<>();
+        for (ArucoDetectionResult detection : tagDetectionPipeResult.output) {
+            // TODO this should be in a pipe, not in the top level here (Matt)
+
+            // 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 correctedBestPose = target.getBestCameraToTarget3d();
+
+            target.setBestCameraToTarget3d(
+                    new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
+
+            targetList.add(target);
+        }
+
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
+        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, frame);
+    }
+}

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

@@ -0,0 +1,42 @@
+/*
+ * 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 org.photonvision.vision.target.TargetModel;
+
+@JsonTypeName("ArucoPipelineSettings")
+public class ArucoPipelineSettings extends AdvancedPipelineSettings {
+    public double decimate = 1;
+    public int threads = 2;
+    public int numIterations = 100;
+    public double cornerAccuracy = 25.0;
+    public boolean useAruco3 = true;
+
+    // 3d settings
+
+    public ArucoPipelineSettings() {
+        super();
+        pipelineType = PipelineType.Aruco;
+        outputShowMultipleTargets = true;
+        targetModel = TargetModel.kAruco6in_16h5;
+        cameraExposure = -1;
+        cameraAutoExposure = true;
+        ledMode = false;
+    }
+}

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

@@ -31,7 +31,8 @@ import org.photonvision.vision.opencv.ImageRotationMode;
     @JsonSubTypes.Type(value = ColoredShapePipelineSettings.class),
     @JsonSubTypes.Type(value = ReflectivePipelineSettings.class),
     @JsonSubTypes.Type(value = DriverModePipelineSettings.class),
-    @JsonSubTypes.Type(value = AprilTagPipelineSettings.class)
+    @JsonSubTypes.Type(value = AprilTagPipelineSettings.class),
+    @JsonSubTypes.Type(value = ArucoPipelineSettings.class)
 })
 public class CVPipelineSettings implements Cloneable {
     public int pipelineIndex = 0;

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

@@ -24,6 +24,7 @@ import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.DualOffsetValues;
 import org.photonvision.vision.pipe.impl.*;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TargetModel;
 import org.photonvision.vision.target.TrackedTarget;
 
 /**
@@ -37,6 +38,9 @@ public class OutputStreamPipeline {
     private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
     private final Draw2dAprilTagsPipe draw2dAprilTagsPipe = new Draw2dAprilTagsPipe();
     private final Draw3dAprilTagsPipe draw3dAprilTagsPipe = new Draw3dAprilTagsPipe();
+
+    private final Draw2dArucoPipe draw2dArucoPipe = new Draw2dArucoPipe();
+    private final Draw3dArucoPipe draw3dArucoPipe = new Draw3dArucoPipe();
     private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
     private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
 
@@ -65,6 +69,13 @@ public class OutputStreamPipeline {
                         settings.streamingFrameDivisor);
         draw2dAprilTagsPipe.setParams(draw2DAprilTagsParams);
 
+        var draw2DArucoParams =
+                new Draw2dArucoPipe.Draw2dArucoParams(
+                        settings.outputShouldDraw,
+                        settings.outputShowMultipleTargets,
+                        settings.streamingFrameDivisor);
+        draw2dArucoPipe.setParams(draw2DArucoParams);
+
         var draw2dCrosshairParams =
                 new Draw2dCrosshairPipe.Draw2dCrosshairParams(
                         settings.outputShouldDraw,
@@ -92,6 +103,14 @@ public class OutputStreamPipeline {
                         settings.streamingFrameDivisor);
         draw3dAprilTagsPipe.setParams(draw3dAprilTagsParams);
 
+        var draw3dArucoParams =
+                new Draw3dArucoPipe.Draw3dArucoParams(
+                        settings.outputShouldDraw,
+                        frameStaticProperties.cameraCalibration,
+                        TargetModel.k6in_16h5,
+                        settings.streamingFrameDivisor);
+        draw3dArucoPipe.setParams(draw3dArucoParams);
+
         resizeImagePipe.setParams(
                 new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
     }
@@ -126,12 +145,12 @@ public class OutputStreamPipeline {
             }
 
             // Draw 2D Crosshair on output
-            var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
+            var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
             sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
 
-            if (!(settings instanceof AprilTagPipelineSettings)) {
-                // If we're processing anything other than Apriltags...
-
+            if (!(settings instanceof AprilTagPipelineSettings)
+                    && !(settings instanceof ArucoPipelineSettings)) {
+                // If we're processing anything other than Apriltags..
                 var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
                 sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
 
@@ -154,7 +173,7 @@ public class OutputStreamPipeline {
                     pipeProfileNanos[8] = 0;
                 }
 
-            } else {
+            } else if (settings instanceof AprilTagPipelineSettings) {
                 // If we are doing apriltags...
                 if (settings.solvePNPEnabled) {
                     // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
@@ -171,6 +190,26 @@ public class OutputStreamPipeline {
                     var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
                     sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
 
+                    pipeProfileNanos[6] = 0;
+                    pipeProfileNanos[7] = 0;
+                    pipeProfileNanos[8] = 0;
+                }
+            } else if (settings instanceof ArucoPipelineSettings) {
+                if (settings.solvePNPEnabled) {
+                    // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
+                    pipeProfileNanos[5] = 0;
+                    pipeProfileNanos[6] = 0;
+
+                    var drawOnInputResult = draw3dArucoPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+
+                    pipeProfileNanos[8] = 0;
+
+                } else {
+                    // Draw 2d apriltag markers
+                    var draw2dTargetsOnInput = draw2dArucoPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+
                     pipeProfileNanos[6] = 0;
                     pipeProfileNanos[7] = 0;
                     pipeProfileNanos[8] = 0;

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

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

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

@@ -21,6 +21,7 @@ import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Comparator;
 import java.util.List;
+import org.opencv.aruco.Aruco;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.dataflow.DataChangeService;
@@ -205,6 +206,11 @@ public class PipelineManager {
                     currentUserPipeline =
                             new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings);
                     break;
+
+                case Aruco:
+                    logger.debug("Creating Aruco Pipeline");
+                    currentUserPipeline = new ArucoPipeline((ArucoPipelineSettings) desiredPipelineSettings);
+                    break;
                 default:
                     // Can be calib3d or drivermode, both of which are special cases
                     break;
@@ -300,6 +306,12 @@ public class PipelineManager {
                     added.pipelineNickname = nickname;
                     return added;
                 }
+            case Aruco:
+                {
+                    var added = new ArucoPipelineSettings();
+                    added.pipelineNickname = nickname;
+                    return added;
+                }
             default:
                 {
                     logger.error("Got invalid pipeline type: " + type.toString());

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

@@ -109,6 +109,14 @@ public enum TargetModel implements Releasable {
                     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)),
+    kAruco6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
+            // do not
+            List.of(
+                    new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0),
+                    new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0),
+                    new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0),
+                    new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)),
+            Units.inchesToMeters(3 * 2)),
     k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
             // do not
             List.of(

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

@@ -18,7 +18,10 @@ package org.photonvision.vision.target;
 
 import edu.wpi.first.apriltag.AprilTagDetection;
 import edu.wpi.first.apriltag.AprilTagPoseEstimate;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import java.util.HashMap;
 import java.util.List;
 import org.opencv.core.CvType;
@@ -28,6 +31,7 @@ import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
 import org.opencv.core.RotatedRect;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.*;
 
@@ -135,6 +139,59 @@ public class TrackedTarget implements Releasable {
         setCameraRelativeRvec(rvec);
     }
 
+    public TrackedTarget(ArucoDetectionResult 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);
+
+        double[] xCorners = result.getxCorners();
+        double[] yCorners = result.getyCorners();
+
+        Point[] cornerPoints =
+                new Point[] {
+                    new Point(xCorners[0], yCorners[0]),
+                    new Point(xCorners[1], yCorners[1]),
+                    new Point(xCorners[2], yCorners[2]),
+                    new Point(xCorners[3], yCorners[3])
+                };
+        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
+
+        var tvec = new Mat(3, 1, CvType.CV_64FC1);
+        tvec.put(0, 0, result.getTvec());
+        setCameraRelativeTvec(tvec);
+
+        var rvec = new Mat(3, 1, CvType.CV_64FC1);
+        rvec.put(0, 0, result.getRvec());
+        setCameraRelativeRvec(rvec);
+
+        {
+            Translation3d translation =
+                    // new Translation3d(tVec.get(0, 0)[0], tVec.get(1, 0)[0], tVec.get(2, 0)[0]);
+                    new Translation3d(result.getTvec()[0], result.getTvec()[1], result.getTvec()[2]);
+            var axisangle =
+                    VecBuilder.fill(result.getRvec()[0], result.getRvec()[1], result.getRvec()[2]);
+            Rotation3d rotation = new Rotation3d(axisangle, axisangle.normF());
+            Transform3d targetPose =
+                    MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation));
+
+            m_bestCameraToTarget3d = targetPose;
+        }
+    }
+
     public void setFiducialId(int id) {
         m_fiducialId = id;
     }

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

@@ -0,0 +1,86 @@
+/*
+ * 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 java.io.IOException;
+import java.util.stream.Collectors;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.photonvision.common.util.TestUtils;
+import org.photonvision.vision.camera.QuirkyCamera;
+import org.photonvision.vision.frame.provider.FileFrameProvider;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TargetModel;
+import org.photonvision.vision.target.TrackedTarget;
+
+public class ArucoPipelineTest {
+    @BeforeEach
+    public void Init() throws IOException {
+        TestUtils.loadLibraries();
+    }
+
+    @Test
+    public void testApriltagFacingCameraAruco() {
+        var pipeline = new ArucoPipeline();
+
+        pipeline.getSettings().inputShouldShow = true;
+        pipeline.getSettings().outputShouldDraw = true;
+        pipeline.getSettings().solvePNPEnabled = true;
+        pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
+        pipeline.getSettings().cornerDetectionUseConvexHulls = true;
+        pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+
+        // pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_16h5_1280, false),
+                        106,
+                        TestUtils.getCoeffs("laptop_1280.json", false));
+        frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
+
+        CVPipelineResult pipelineResult;
+        try {
+            pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+            printTestResults(pipelineResult);
+        } catch (RuntimeException e) {
+            // For now, will throw coz rotation3d ctor
+            return;
+        }
+
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        outputPipe.process(
+                pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
+
+        TestUtils.showImage(
+                pipelineResult.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
+    }
+
+    private static void printTestResults(CVPipelineResult pipelineResult) {
+        double fps = 1000 / pipelineResult.getLatencyMillis();
+        System.out.println(
+                "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)");
+        System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
+        System.out.println(
+                "Found targets at "
+                        + pipelineResult.targets.stream()
+                                .map(TrackedTarget::getBestCameraToTarget3d)
+                                .collect(Collectors.toList()));
+    }
+}