Re-enable OpenCV ArUco detector for AprilTags (#916)

- Fixes ArUco on picam
- Adds `ArucoPoseEstimatorPipe` for single-tag pose estimation
  - Previously, `Aruco.estimatePoseSingleMarkers()` was used for tag pose estimation. This uses the default `SOLVEPNP_ITERATIVE` solver and I believe the method is removed in opencv 4.8. The `SOLVEPNP_IPPE_SQUARE` solver implemented is more appropriate for markers.
- Pipeline architecture cleanup
- Re-enables ArUco pipeline in UI
- Multi-tag support

ArUco detector support is still considered experimental at this time. This should enable a baseline of support for initial testing, but expect some quirks to remain across platforms.

photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue

@@ -151,8 +151,8 @@ const pipelineTypesWrapper = computed<{ name: string; value: number }[]>(() => {
   const pipelineTypes = [
     { name: "Reflective", value: WebsocketPipelineType.Reflective },
     { name: "Colored Shape", value: WebsocketPipelineType.ColoredShape },
-    { name: "AprilTag", value: WebsocketPipelineType.AprilTag }
+    { name: "AprilTag", value: WebsocketPipelineType.AprilTag },
-    // { name: "Aruco", value: WebsocketPipelineType.Aruco }
+    { name: "Aruco", value: WebsocketPipelineType.Aruco }
   ];
 
   if (useCameraSettingsStore().isDriverMode) {
@@ -353,8 +353,8 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
             :items="[
               { name: 'Reflective', value: WebsocketPipelineType.Reflective },
               { name: 'Colored Shape', value: WebsocketPipelineType.ColoredShape },
-              { name: 'AprilTag', value: WebsocketPipelineType.AprilTag }
+              { name: 'AprilTag', value: WebsocketPipelineType.AprilTag },
-              // { name: 'Aruco', value: WebsocketPipelineType.Aruco }
+              { name: 'Aruco', value: WebsocketPipelineType.Aruco }
             ]"
           />
         </v-card-text>

photon-client/src/components/dashboard/tabs/ArucoTab.vue

@@ -2,6 +2,9 @@
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { PipelineType } from "@/types/PipelineTypes";
 import PvSlider from "@/components/common/pv-slider.vue";
+import PvSwitch from "@/components/common/pv-switch.vue";
+import PvRangeSlider from "@/components/common/pv-range-slider.vue";
+import PvSelect from "@/components/common/pv-select.vue";
 import { computed, getCurrentInstance } from "vue";
 import { useStateStore } from "@/stores/StateStore";
 
@@ -20,37 +23,60 @@ const interactiveCols = computed(
 
 <template>
   <div v-if="currentPipelineSettings.pipelineType === PipelineType.Aruco">
-    <pv-slider
+    <pv-select
-      v-model="currentPipelineSettings.decimate"
+      v-model="currentPipelineSettings.tagFamily"
+      label="Target family"
+      :items="['AprilTag Family 36h11', 'AprilTag Family 25h9', 'AprilTag Family 16h5']"
+      :select-cols="interactiveCols"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ tagFamily: value }, false)"
+    />
+    <pv-switch
+      v-model="currentPipelineSettings.useCornerRefinement"
       class="pt-2"
+      label="Refine Corners"
+      tooltip="Further refine the initial corners with subpixel accuracy."
+      :switch-cols="interactiveCols"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ useCornerRefinement: value }, false)"
+    />
+    <pv-range-slider
+      v-model="currentPipelineSettings.threshWinSizes"
+      label="Thresh Min/Max Size"
+      tooltip="The minimum and maximum adaptive threshold window size. Larger windows tend more towards global thresholding, but small windows can be weak to noise."
+      :min="3"
+      :max="255"
       :slider-cols="interactiveCols"
-      label="Decimate"
+      :step="2"
-      tooltip="Increases FPS at the expense of range by reducing image resolution initially"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ threshWinSizes: value }, false)"
-      :min="1"
-      :max="8"
-      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ decimate: value }, false)"
     />
     <pv-slider
-      v-model="currentPipelineSettings.numIterations"
+      v-model="currentPipelineSettings.threshStepSize"
       class="pt-2"
       :slider-cols="interactiveCols"
-      label="Corner Iterations"
+      label="Thresh Step Size"
-      tooltip="How many iterations are going to be used in order to refine corners. Higher values are lead to more accuracy at the cost of performance"
+      tooltip="Smaller values will cause more steps between the min/max sizes. More, varied steps can improve detection robustness to lighting, but may decrease performance."
-      :min="30"
+      :min="2"
-      :max="1000"
+      :max="128"
-      :step="5"
+      :step="1"
-      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ numIterations: value }, false)"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ threshStepSize: value }, false)"
     />
     <pv-slider
-      v-model="currentPipelineSettings.cornerAccuracy"
+      v-model="currentPipelineSettings.threshConstant"
       class="pt-2"
       :slider-cols="interactiveCols"
-      label="Corner Accuracy"
+      label="Thresh Constant"
-      tooltip="Minimum accuracy for the corners, lower is better but more performance intensive "
+      tooltip="Affects the threshold window mean value cutoff for all steps. Higher values can improve performance, but may harm detection rate."
-      :min="0.01"
+      :min="0"
-      :max="100"
+      :max="128"
-      :step="0.01"
+      :step="1"
-      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ cornerAccuracy: value }, false)"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ threshConstant: value }, false)"
+    />
+    <pv-switch
+      v-model="currentPipelineSettings.debugThreshold"
+      class="pt-2"
+      label="Debug Threshold"
+      tooltip="Display the first threshold step to the color stream."
+      :switch-cols="interactiveCols"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ debugThreshold: value }, false)"
     />
   </div>
 </template>

photon-client/src/components/dashboard/tabs/OutputTab.vue

@@ -86,7 +86,8 @@ const interactiveCols = computed(
     />
     <pv-switch
       v-if="
-        currentPipelineSettings.pipelineType === PipelineType.AprilTag &&
+        (currentPipelineSettings.pipelineType === PipelineType.AprilTag ||
+          currentPipelineSettings.pipelineType === PipelineType.Aruco) &&
         useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
         useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled
       "
@@ -99,7 +100,8 @@ const interactiveCols = computed(
     />
     <pv-switch
       v-if="
-        currentPipelineSettings.pipelineType === PipelineType.AprilTag &&
+        (currentPipelineSettings.pipelineType === PipelineType.AprilTag ||
+          currentPipelineSettings.pipelineType === PipelineType.Aruco) &&
         useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
         useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled
       "

photon-client/src/components/dashboard/tabs/TargetsTab.vue

@@ -36,7 +36,8 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings
               </template>
               <template
                 v-if="
-                  useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag &&
+                  (useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag ||
+                    useCameraSettingsStore().currentPipelineType === PipelineType.Aruco) &&
                   useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled
                 "
               >
@@ -67,7 +68,8 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings
               </template>
               <template
                 v-if="
-                  useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag &&
+                  (useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag ||
+                    useCameraSettingsStore().currentPipelineType === PipelineType.Aruco) &&
                   useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled
                 "
               >
@@ -80,7 +82,8 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings
     </v-row>
     <v-row
       v-if="
-        currentPipelineSettings.pipelineType === PipelineType.AprilTag &&
+        (useCameraSettingsStore().currentPipelineType === PipelineType.AprilTag ||
+          useCameraSettingsStore().currentPipelineType === PipelineType.Aruco) &&
         currentPipelineSettings.doMultiTarget &&
         useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
         useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled

photon-client/src/types/PipelineTypes.ts

@@ -245,11 +245,22 @@ export const DefaultAprilTagPipelineSettings: AprilTagPipelineSettings = {
 
 export interface ArucoPipelineSettings extends PipelineSettings {
   pipelineType: PipelineType.Aruco;
-  decimate: number;
+
-  threads: number;
+  tagFamily: AprilTagFamily;
-  numIterations: number;
+
-  cornerAccuracy: number;
+  threshWinSizes: WebsocketNumberPair | [number, number];
+  threshStepSize: number;
+  threshConstant: number;
+  debugThreshold: boolean;
+
+  useCornerRefinement: boolean;
+
   useAruco3: boolean;
+  aruco3MinMarkerSideRatio: number;
+  aruco3MinCanonicalImgSide: number;
+
+  doMultiTarget: boolean;
+  doSingleTargetAlways: boolean;
 }
 export type ConfigurableArucoPipelineSettings = Partial<Omit<ArucoPipelineSettings, "pipelineType">> &
   ConfigurablePipelineSettings;
@@ -262,11 +273,17 @@ export const DefaultArucoPipelineSettings: ArucoPipelineSettings = {
   ledMode: false,
   pipelineType: PipelineType.Aruco,
 
-  decimate: 1,
+  tagFamily: AprilTagFamily.Family16h5,
-  threads: 2,
+  threshWinSizes: { first: 11, second: 91 },
-  numIterations: 100,
+  threshStepSize: 40,
-  cornerAccuracy: 25,
+  threshConstant: 10,
-  useAruco3: true
+  debugThreshold: false,
+  useCornerRefinement: true,
+  useAruco3: false,
+  aruco3MinMarkerSideRatio: 0.02,
+  aruco3MinCanonicalImgSide: 32,
+  doMultiTarget: false,
+  doSingleTargetAlways: false
 };
 
 export type ActivePipelineSettings =

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

@@ -37,7 +37,6 @@ import org.photonvision.vision.processes.VisionSource;
 import org.zeroturnaround.zip.ZipUtil;
 
 public class ConfigManager {
-    private static final Logger logger = new Logger(ConfigManager.class, LogGroup.General);
     private static ConfigManager INSTANCE;
 
     public static final String HW_CFG_FNAME = "hardwareConfig.json";
@@ -79,6 +78,8 @@ public class ConfigManager {
         return INSTANCE;
     }
 
+    private static final Logger logger = new Logger(ConfigManager.class, LogGroup.General);
+
     private void translateLegacyIfPresent(Path folderPath) {
         if (!(m_provider instanceof SqlConfigProvider)) {
             // Cannot import into SQL if we aren't in SQL mode rn

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

@@ -24,36 +24,24 @@ 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;
+    private final double[] xCorners;
+    private final double[] yCorners;
 
-    double[] tvec, rvec;
+    private final int id;
 
-    public ArucoDetectionResult(
+    public ArucoDetectionResult(double[] xCorners, double[] yCorners, int id) {
-            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() {
+    public double[] getXCorners() {
-        return tvec;
-    }
-
-    public double[] getRvec() {
-        return rvec;
-    }
-
-    public double[] getxCorners() {
         return xCorners;
     }
 
-    public double[] getyCorners() {
+    public double[] getYCorners() {
         return yCorners;
     }
 
@@ -62,11 +50,11 @@ public class ArucoDetectionResult {
     }
 
     public double getCenterX() {
-        return (xCorners[0] + xCorners[1] + xCorners[2] + xCorners[3]) * .25;
+        return (xCorners[0] + xCorners[1] + xCorners[2] + xCorners[3]) / 4.0;
     }
 
     public double getCenterY() {
-        return (yCorners[0] + yCorners[1] + yCorners[2] + yCorners[3]) * .25;
+        return (yCorners[0] + yCorners[1] + yCorners[2] + yCorners[3]) / 4.0;
     }
 
     @Override

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

@@ -1,77 +0,0 @@
-/*
- * 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.objdetect.ArucoDetector;
-import org.opencv.objdetect.DetectorParameters;
-import org.opencv.objdetect.Objdetect;
-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 = new DetectorParameters();
-    ArucoDetector detector;
-
-    public ArucoDetectorParams() {
-        setDecimation(1);
-        setCornerAccuracy(25);
-        setCornerRefinementMaxIterations(100);
-
-        detector =
-                new ArucoDetector(
-                        Objdetect.getPredefinedDictionary(Objdetect.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 pointer to the parameters
-        // object internally, so it should automatically update
-        parameters.set_aprilTagQuadDecimate(decimate);
-        m_decimate = decimate;
-    }
-
-    public void setCornerRefinementMaxIterations(int iters) {
-        if (iters == m_iterations || iters <= 0) return;
-
-        parameters.set_cornerRefinementMethod(Objdetect.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

@@ -17,116 +17,82 @@
 
 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 java.util.Arrays;
+import java.util.Comparator;
 import org.opencv.core.Mat;
 import org.opencv.objdetect.ArucoDetector;
+import org.opencv.objdetect.DetectorParameters;
+import org.opencv.objdetect.Objdetect;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
-import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 
+/** This class wraps an {@link ArucoDetector} for convenience. */
 public class PhotonArucoDetector {
     private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
 
-    private static final Rotation3d ARUCO_BASE_ROTATION =
+    private final ArucoDetector detector =
-            new Rotation3d(VecBuilder.fill(0, 0, 1), Units.degreesToRadians(180));
+            new ArucoDetector(Objdetect.getPredefinedDictionary(Objdetect.DICT_APRILTAG_16h5));
 
-    Mat ids;
+    private final Mat ids = new Mat();
+    private final ArrayList<Mat> cornerMats = new ArrayList<>();
 
-    Mat tvecs;
+    public ArucoDetector getDetector() {
-    Mat rvecs;
+        return detector;
-    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<>();
-        tagPose = new Pose3d();
-        translation = new Translation3d();
-        rotation = new Rotation3d();
     }
 
-    public ArucoDetectionResult[] detect(
+    /**
-            Mat grayscaleImg,
+     * Get a copy of the current parameters being used. Must next call setParams to update the
-            float tagSize,
+     * underlying detector object!
-            CameraCalibrationCoefficients coeffs,
+     */
-            ArucoDetector detector) {
+    public DetectorParameters getParams() {
-        detector.detectMarkers(grayscaleImg, corners, ids);
+        return detector.getDetectorParameters();
-        if (coeffs != null) {
+    }
-            Aruco.estimatePoseSingleMarkers(
-                    corners,
-                    tagSize,
-                    coeffs.getCameraIntrinsicsMat(),
-                    coeffs.getDistCoeffsMat(),
-                    rvecs,
-                    tvecs);
-        }
 
-        ArucoDetectionResult[] toReturn = new ArucoDetectionResult[corners.size()];
+    public void setParams(DetectorParameters params) {
-        timeStartProcess = System.currentTimeMillis();
+        detector.setDetectorParameters(params);
-        for (int i = 0; i < corners.size(); i++) {
+    }
-            cornerMat = corners.get(i);
+
-            // logger.debug(cornerMat.dump());
+    /**
-            xCorners =
+     * Detect fiducial tags in the grayscaled image using the {@link ArucoDetector} in this class.
-                    new double[] {
+     * Parameters for detection can be modified with {@link #setParams(DetectorParameters)}.
-                        cornerMat.get(0, 0)[0],
+     *
-                        cornerMat.get(0, 1)[0],
+     * @param grayscaleImg A grayscaled image
-                        cornerMat.get(0, 2)[0],
+     * @return An array of ArucoDetectionResult, which contain tag corners and id.
-                        cornerMat.get(0, 3)[0]
+     */
-                    };
+    public ArucoDetectionResult[] detect(Mat grayscaleImg) {
-            yCorners =
+        // detect tags
-                    new double[] {
+        detector.detectMarkers(grayscaleImg, cornerMats, ids);
-                        cornerMat.get(0, 0)[1],
+
-                        cornerMat.get(0, 1)[1],
+        ArucoDetectionResult[] results = new ArucoDetectionResult[cornerMats.size()];
-                        cornerMat.get(0, 2)[1],
+        for (int i = 0; i < cornerMats.size(); i++) {
-                        cornerMat.get(0, 3)[1]
+            // each detection has a Mat of corners
-                    };
+            Mat cornerMat = cornerMats.get(i);
+
+            // Aruco detection returns corners (BR, BL, TL, TR).
+            // For parity with AprilTags and photonlib, we want (BL, BR, TR, TL).
+            double[] xCorners = {
+                cornerMat.get(0, 1)[0],
+                cornerMat.get(0, 0)[0],
+                cornerMat.get(0, 3)[0],
+                cornerMat.get(0, 2)[0]
+            };
+            double[] yCorners = {
+                cornerMat.get(0, 1)[1],
+                cornerMat.get(0, 0)[1],
+                cornerMat.get(0, 3)[1],
+                cornerMat.get(0, 2)[1]
+            };
             cornerMat.release();
 
-            double[] tvec;
+            results[i] = new ArucoDetectionResult(xCorners, yCorners, (int) ids.get(i, 0)[0]);
-            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;
+        // sort tags by ID
+        Arrays.sort(results, Comparator.comparingInt(ArucoDetectionResult::getId));
+
+        return results;
     }
 }

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

@@ -17,34 +17,111 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.util.Units;
 import java.util.List;
 import org.opencv.core.Mat;
-import org.opencv.objdetect.DetectorParameters;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.core.TermCriteria;
+import org.opencv.imgproc.Imgproc;
+import org.opencv.objdetect.Objdetect;
 import org.photonvision.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.aruco.PhotonArucoDetector;
+import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.pipe.CVPipe;
 
 public class ArucoDetectionPipe
-        extends CVPipe<Mat, List<ArucoDetectionResult>, ArucoDetectionPipeParams> {
+        extends CVPipe<CVMat, List<ArucoDetectionResult>, ArucoDetectionPipeParams> {
-    PhotonArucoDetector detector = new PhotonArucoDetector();
+    // ArucoDetector wrapper class
+    private final PhotonArucoDetector photonDetector = new PhotonArucoDetector();
+
+    // Ratio multiplied with image size and added to refinement window size
+    private static final double kRefineWindowImageRatio = 0.004;
+    // Ratio multiplied with max marker diagonal length and added to refinement window size
+    private static final double kRefineWindowMarkerRatio = 0.03;
 
     @Override
-    protected List<ArucoDetectionResult> process(Mat in) {
+    protected List<ArucoDetectionResult> process(CVMat in) {
-        return List.of(
+        var imgMat = in.getMat();
-                detector.detect(
+        var detections = photonDetector.detect(imgMat);
-                        in,
+        // manually do corner refinement ourselves
-                        (float) Units.inchesToMeters(6),
+        if (params.useCornerRefinement) {
-                        params.cameraCalibrationCoefficients,
+            for (var detection : detections) {
-                        params.detectorParams));
+                double[] xCorners = detection.getXCorners();
+                double[] yCorners = detection.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])
+                        };
+                double bltr =
+                        Math.hypot(
+                                cornerPoints[2].x - cornerPoints[0].x, cornerPoints[2].y - cornerPoints[0].y);
+                double brtl =
+                        Math.hypot(
+                                cornerPoints[3].x - cornerPoints[1].x, cornerPoints[3].y - cornerPoints[1].y);
+                double minDiag = Math.min(bltr, brtl);
+                int halfWindowLength =
+                        (int) Math.ceil(kRefineWindowImageRatio * Math.min(imgMat.rows(), imgMat.cols()));
+                halfWindowLength += (int) (minDiag * kRefineWindowMarkerRatio);
+                // dont do refinement on small markers
+                if (halfWindowLength < 4) continue;
+                var halfWindowSize = new Size(halfWindowLength, halfWindowLength);
+                var ptsMat = new MatOfPoint2f(cornerPoints);
+                var criteria =
+                        new TermCriteria(3, params.refinementMaxIterations, params.refinementMinErrorPx);
+                Imgproc.cornerSubPix(imgMat, ptsMat, halfWindowSize, new Size(-1, -1), criteria);
+                cornerPoints = ptsMat.toArray();
+                for (int i = 0; i < cornerPoints.length; i++) {
+                    var pt = cornerPoints[i];
+                    xCorners[i] = pt.x;
+                    yCorners[i] = pt.y;
+                    // If we want to debug the refinement window, draw a rectangle on the image
+                    if (params.debugRefineWindow) {
+                        drawCornerRefineWindow(imgMat, pt, halfWindowLength);
+                    }
+                }
+            }
+        }
+        return List.of(detections);
     }
 
     @Override
-    public void setParams(ArucoDetectionPipeParams params) {
+    public void setParams(ArucoDetectionPipeParams newParams) {
-        super.setParams(params);
+        if (this.params == null || !this.params.equals(newParams)) {
+            photonDetector
+                    .getDetector()
+                    .setDictionary(Objdetect.getPredefinedDictionary(newParams.tagFamily));
+            var detectParams = photonDetector.getParams();
+
+            detectParams.set_adaptiveThreshWinSizeMin(newParams.threshMinSize);
+            detectParams.set_adaptiveThreshWinSizeStep(newParams.threshStepSize);
+            detectParams.set_adaptiveThreshWinSizeMax(newParams.threshMaxSize);
+            detectParams.set_adaptiveThreshConstant(newParams.threshConstant);
+
+            detectParams.set_errorCorrectionRate(newParams.errorCorrectionRate);
+
+            detectParams.set_useAruco3Detection(newParams.useAruco3);
+            detectParams.set_minSideLengthCanonicalImg(newParams.aruco3MinCanonicalImgSide);
+            detectParams.set_minMarkerLengthRatioOriginalImg((float) newParams.aruco3MinMarkerSideRatio);
+
+            photonDetector.setParams(detectParams);
+        }
+
+        super.setParams(newParams);
     }
 
-    public DetectorParameters getParameters() {
+    public PhotonArucoDetector getPhotonDetector() {
-        return params == null ? null : params.detectorParams.getDetectorParameters();
+        return photonDetector;
+    }
+
+    private void drawCornerRefineWindow(Mat outputMat, Point corner, int windowSize) {
+        int thickness = (int) (Math.ceil(Math.max(outputMat.cols(), outputMat.rows()) * 0.003));
+        var pt1 = new Point(corner.x - windowSize, corner.y - windowSize);
+        var pt2 = new Point(corner.x + windowSize, corner.y + windowSize);
+        Imgproc.rectangle(outputMat, pt1, pt2, new Scalar(0, 0, 255), thickness);
     }
 }

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

@@ -18,40 +18,90 @@
 package org.photonvision.vision.pipe.impl;
 
 import java.util.Objects;
-import org.opencv.objdetect.ArucoDetector;
+import org.opencv.objdetect.Objdetect;
-import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 
+/** Detector parameters. See https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html. */
 public class ArucoDetectionPipeParams {
-    public ArucoDetector detectorParams;
+    /** Tag family. Default: {@link Objdetect#DICT_APRILTAG_16h5}. */
-    public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
+    public int tagFamily = Objdetect.DICT_APRILTAG_16h5;
 
-    public ArucoDetectionPipeParams(
+    public int threshMinSize = 11;
-            ArucoDetector detector, CameraCalibrationCoefficients cameraCalibrationCoefficients) {
+    public int threshStepSize = 40;
-        this.detectorParams = detector;
+    public int threshMaxSize = 91;
-        this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
+    public double threshConstant = 10;
-    }
+
+    /**
+     * Bits allowed to be corrected, expressed as a ratio of the tag families theoretical maximum.
+     *
+     * <p>E.g. 36h11 -> 11 * errorCorrectionRate = Max error bits
+     */
+    public double errorCorrectionRate = 0;
+
+    /**
+     * If obtained corners should be iteratively refined. This should always be on for 3D estimation.
+     */
+    public boolean useCornerRefinement = true;
+
+    /** Maximum corner refinement iterations. */
+    public int refinementMaxIterations = 30;
+
+    /**
+     * Minimum error (accuracy) for corner refinement in pixels. When a corner refinement iteration
+     * moves the corner by less than this value, the refinement is considered finished.
+     */
+    public double refinementMinErrorPx = 0.005;
+
+    public boolean debugRefineWindow = false;
+
+    /**
+     * If the 'Aruco3' speedup should be used. This is similar to AprilTag's 'decimate' value, but
+     * automatically determined with the given parameters.
+     *
+     * <p>T_i = aruco3MinMarkerSideRatio, and T_c = aruco3MinCanonicalImgSide
+     *
+     * <p>Scale factor = T_c / (T_c + T_i * max(img_width, img_height))
+     */
+    public boolean useAruco3 = false;
+
+    /** Minimum side length of markers expressed as a ratio of the largest image dimension. */
+    public double aruco3MinMarkerSideRatio = 0.02;
+
+    /** Minimum side length of the canonical image (marker after undoing perspective distortion). */
+    public int aruco3MinCanonicalImgSide = 32;
 
     @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)
+        return tagFamily == that.tagFamily
-                && Objects.equals(cameraCalibrationCoefficients, that.cameraCalibrationCoefficients);
+                && threshMinSize == that.threshMinSize
+                && threshStepSize == that.threshStepSize
+                && threshMaxSize == that.threshMaxSize
+                && threshConstant == that.threshConstant
+                && errorCorrectionRate == that.errorCorrectionRate
+                && useCornerRefinement == that.useCornerRefinement
+                && refinementMaxIterations == that.refinementMaxIterations
+                && refinementMinErrorPx == that.refinementMinErrorPx
+                && useAruco3 == that.useAruco3
+                && aruco3MinMarkerSideRatio == that.aruco3MinMarkerSideRatio
+                && aruco3MinCanonicalImgSide == that.aruco3MinCanonicalImgSide;
     }
 
     @Override
     public int hashCode() {
-        return Objects.hash(detectorParams, cameraCalibrationCoefficients);
+        return Objects.hash(
-    }
+                tagFamily,
-
+                threshMinSize,
-    @Override
+                threshStepSize,
-    public String toString() {
+                threshMaxSize,
-        return "ArucoDetectionPipeParams{"
+                threshConstant,
-                + "detectorParams="
+                errorCorrectionRate,
-                + detectorParams
+                useCornerRefinement,
-                + ", cameraCalibrationCoefficients="
+                refinementMaxIterations,
-                + cameraCalibrationCoefficients
+                refinementMinErrorPx,
-                + '}';
+                useAruco3,
+                aruco3MinMarkerSideRatio,
+                aruco3MinCanonicalImgSide);
     }
 }

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

@@ -0,0 +1,140 @@
+/*
+ * 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.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.ArrayList;
+import java.util.List;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point3;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class ArucoPoseEstimatorPipe
+        extends CVPipe<
+                ArucoDetectionResult,
+                AprilTagPoseEstimate,
+                ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams> {
+    // image points of marker corners
+    private final MatOfPoint2f imagePoints = new MatOfPoint2f(Mat.zeros(4, 1, CvType.CV_32FC2));
+    // rvec/tvec estimations from solvepnp
+    private final List<Mat> rvecs = new ArrayList<>();
+    private final List<Mat> tvecs = new ArrayList<>();
+    // unused parameters
+    private final Mat rvec = Mat.zeros(3, 1, CvType.CV_32F);
+    private final Mat tvec = Mat.zeros(3, 1, CvType.CV_32F);
+    // reprojection error of solvepnp estimations
+    private final Mat reprojectionErrors = Mat.zeros(2, 1, CvType.CV_32F);
+
+    private final int kNaNRetries = 1;
+
+    private Translation3d tvecToTranslation3d(Mat mat) {
+        double[] tvec = new double[3];
+        mat.get(0, 0, tvec);
+        return new Translation3d(tvec[0], tvec[1], tvec[2]);
+    }
+
+    private Rotation3d rvecToRotation3d(Mat mat) {
+        double[] rvec = new double[3];
+        mat.get(0, 0, rvec);
+        return new Rotation3d(VecBuilder.fill(rvec[0], rvec[1], rvec[2]));
+    }
+
+    @Override
+    protected AprilTagPoseEstimate process(ArucoDetectionResult in) {
+        // We receive 2d corners as (BL, BR, TR, TL) but we want (BR, BL, TL, TR)
+        double[] xCorn = in.getXCorners();
+        double[] yCorn = in.getYCorners();
+        imagePoints.put(0, 0, new float[] {(float) xCorn[1], (float) yCorn[1]});
+        imagePoints.put(1, 0, new float[] {(float) xCorn[0], (float) yCorn[0]});
+        imagePoints.put(2, 0, new float[] {(float) xCorn[3], (float) yCorn[3]});
+        imagePoints.put(3, 0, new float[] {(float) xCorn[2], (float) yCorn[2]});
+
+        float[] reprojErrors = new float[2];
+        // very rarely the solvepnp solver returns NaN results, so we retry with slight noise added
+        for (int i = 0; i < kNaNRetries + 1; i++) {
+            // SolvePnP with SOLVEPNP_IPPE_SQUARE solver
+            Calib3d.solvePnPGeneric(
+                    params.objectPoints,
+                    imagePoints,
+                    params.calibration.getCameraIntrinsicsMat(),
+                    params.calibration.getDistCoeffsMat(),
+                    rvecs,
+                    tvecs,
+                    false,
+                    Calib3d.SOLVEPNP_IPPE_SQUARE,
+                    rvec,
+                    tvec,
+                    reprojectionErrors);
+
+            // check if we got a NaN result
+            reprojectionErrors.get(0, 0, reprojErrors);
+            if (!Double.isNaN(reprojErrors[0])) break;
+            else { // add noise and retry
+                double[] br = imagePoints.get(0, 0);
+                br[0] -= 0.001;
+                br[1] -= 0.001;
+                imagePoints.put(0, 0, br);
+            }
+        }
+
+        // create AprilTagPoseEstimate with results
+        if (tvecs.isEmpty())
+            return new AprilTagPoseEstimate(new Transform3d(), new Transform3d(), 0, 0);
+        return new AprilTagPoseEstimate(
+                new Transform3d(tvecToTranslation3d(tvecs.get(0)), rvecToRotation3d(rvecs.get(0))),
+                new Transform3d(tvecToTranslation3d(tvecs.get(1)), rvecToRotation3d(rvecs.get(1))),
+                reprojErrors[0],
+                reprojErrors[1]);
+    }
+
+    @Override
+    public void setParams(ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams newParams) {
+        super.setParams(newParams);
+    }
+
+    public static class ArucoPoseEstimatorPipeParams {
+        final CameraCalibrationCoefficients calibration;
+        final double tagSize;
+        // object vertices defined by tag size
+        final MatOfPoint3f objectPoints;
+
+        public ArucoPoseEstimatorPipeParams(CameraCalibrationCoefficients cal, double tagSize) {
+            this.calibration = cal;
+            this.tagSize = tagSize;
+
+            // This order is relevant for SOLVEPNP_IPPE_SQUARE
+            // The expected 2d correspondences with a tag facing the camera would be (BR, BL, TL, TR)
+            objectPoints =
+                    new MatOfPoint3f(
+                            new Point3(-tagSize / 2, tagSize / 2, 0),
+                            new Point3(tagSize / 2, tagSize / 2, 0),
+                            new Point3(tagSize / 2, -tagSize / 2, 0),
+                            new Point3(-tagSize / 2, -tagSize / 2, 0));
+        }
+    }
+}

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

@@ -125,8 +125,7 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
         long sumPipeNanosElapsed = 0L;
 
         if (frame.type != FrameThresholdType.GREYSCALE) {
-            // TODO so all cameras should give us GREYSCALE -- how should we handle if not?
+            // We asked for a GREYSCALE frame, but didn't get one -- best we can do is give up
-            // Right now, we just return nothing
             return new CVPipelineResult(0, 0, List.of(), frame);
         }
 

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

@@ -32,7 +32,7 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
     public int numIterations = 40;
     public int hammingDist = 0;
     public int decisionMargin = 35;
-    public boolean doMultiTarget = true;
+    public boolean doMultiTarget = false;
     public boolean doSingleTargetAlways = false;
 
     // 3d settings

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

@@ -34,29 +34,36 @@
 
 package org.photonvision.vision.pipeline;
 
+import edu.wpi.first.apriltag.AprilTagPoseEstimate;
+import edu.wpi.first.math.geometry.CoordinateSystem;
+import edu.wpi.first.math.geometry.Pose3d;
 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.opencv.imgproc.Imgproc;
+import org.opencv.objdetect.Objdetect;
+import org.photonvision.common.configuration.ConfigManager;
+import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.targeting.MultiTargetPNPResults;
 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.pipe.impl.ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams;
+import org.photonvision.vision.pipe.impl.MultiTargetPNPPipe.MultiTargetPNPPipeParams;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
 import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
 
 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 ArucoPoseEstimatorPipe singleTagPoseEstimatorPipe = new ArucoPoseEstimatorPipe();
+    private final MultiTargetPNPPipe multiTagPNPPipe = new MultiTargetPNPPipe();
     private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
 
-    ArucoDetectorParams m_arucoDetectorParams = new ArucoDetectorParams();
-
     public ArucoPipeline() {
         super(FrameThresholdType.GREYSCALE);
         settings = new ArucoPipelineSettings();
@@ -69,59 +76,171 @@ public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSet
 
     @Override
     protected void setPipeParamsImpl() {
-        // Sanitize thread count - not supported to have fewer than 1 threads
+        var params = new ArucoDetectionPipeParams();
-        settings.threads = Math.max(1, settings.threads);
+        // sanitize and record settings
 
-        RotateImagePipe.RotateImageParams rotateImageParams =
+        switch (settings.tagFamily) {
-                new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
+            case kTag36h11:
-        rotateImagePipe.setParams(rotateImageParams);
+                params.tagFamily = Objdetect.DICT_APRILTAG_36h11;
+                break;
+            case kTag25h9:
+                params.tagFamily = Objdetect.DICT_APRILTAG_25h9;
+                break;
+            default:
+                params.tagFamily = Objdetect.DICT_APRILTAG_16h5;
+        }
 
-        m_arucoDetectorParams.setDecimation((float) settings.decimate);
+        int threshMinSize = Math.max(3, settings.threshWinSizes.getFirst());
-        m_arucoDetectorParams.setCornerRefinementMaxIterations(settings.numIterations);
+        settings.threshWinSizes.setFirst(threshMinSize);
-        m_arucoDetectorParams.setCornerAccuracy(settings.cornerAccuracy);
+        params.threshMinSize = threshMinSize;
+        int threshStepSize = Math.max(2, settings.threshStepSize);
+        settings.threshStepSize = threshStepSize;
+        params.threshStepSize = threshStepSize;
+        int threshMaxSize = Math.max(threshMinSize, settings.threshWinSizes.getSecond());
+        settings.threshWinSizes.setSecond(threshMaxSize);
+        params.threshMaxSize = threshMaxSize;
+        params.threshConstant = settings.threshConstant;
 
-        arucoDetectionPipe.setParams(
+        params.useCornerRefinement = settings.useCornerRefinement;
-                new ArucoDetectionPipeParams(
+        params.refinementMaxIterations = settings.refineNumIterations;
-                        m_arucoDetectorParams.getDetector(), frameStaticProperties.cameraCalibration));
+        params.refinementMinErrorPx = settings.refineMinErrorPx;
+        params.useAruco3 = settings.useAruco3;
+        params.aruco3MinMarkerSideRatio = settings.aruco3MinMarkerSideRatio;
+        params.aruco3MinCanonicalImgSide = settings.aruco3MinCanonicalImgSide;
+        arucoDetectionPipe.setParams(params);
+
+        if (frameStaticProperties.cameraCalibration != null) {
+            var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
+            if (cameraMatrix != null && cameraMatrix.rows() > 0) {
+                var estimatorParams =
+                        new ArucoPoseEstimatorPipeParams(
+                                frameStaticProperties.cameraCalibration, Units.inchesToMeters(6));
+                singleTagPoseEstimatorPipe.setParams(estimatorParams);
+
+                // TODO global state ew
+                var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
+                multiTagPNPPipe.setParams(
+                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl));
+            }
+        }
     }
 
     @Override
     protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) {
         long sumPipeNanosElapsed = 0L;
-        Mat rawInputMat;
-        rawInputMat = frame.colorImage.getMat();
 
-        List<TrackedTarget> targetList;
+        if (frame.type != FrameThresholdType.GREYSCALE) {
-        CVPipeResult<List<ArucoDetectionResult>> tagDetectionPipeResult;
+            // We asked for a GREYSCALE frame, but didn't get one -- best we can do is give up
-
+            return new CVPipelineResult(0, 0, List.of(), frame);
-        if (rawInputMat.empty()) {
-            return new CVPipelineResult(sumPipeNanosElapsed, 0, List.of(), frame);
         }
 
-        tagDetectionPipeResult = arucoDetectionPipe.run(rawInputMat);
+        CVPipeResult<List<ArucoDetectionResult>> tagDetectionPipeResult;
-        targetList = new ArrayList<>();
+        tagDetectionPipeResult = arucoDetectionPipe.run(frame.processedImage);
-        for (ArucoDetectionResult detection : tagDetectionPipeResult.output) {
+        sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
-            // TODO this should be in a pipe, not in the top level here (Matt)
 
-            // populate the target list
+        // If we want to debug the thresholding steps, draw the first step to the color image
-            // Challenge here is that TrackedTarget functions with OpenCV Contour
+        if (settings.debugThreshold) {
+            drawThresholdFrame(
+                    frame.processedImage.getMat(),
+                    frame.colorImage.getMat(),
+                    settings.threshWinSizes.getFirst(),
+                    settings.threshConstant);
+        }
+
+        List<TrackedTarget> targetList = new ArrayList<>();
+        for (ArucoDetectionResult detection : tagDetectionPipeResult.output) {
+            // Populate target list for multitag
+            // (TODO: Address circular dependencies. Multitag only requires corners and IDs, this should
+            // not be necessary.)
             TrackedTarget target =
                     new TrackedTarget(
                             detection,
+                            null,
                             new TargetCalculationParameters(
                                     false, null, null, null, null, frameStaticProperties));
 
-            var correctedBestPose = target.getBestCameraToTarget3d();
-
-            target.setBestCameraToTarget3d(
-                    new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
-
             targetList.add(target);
         }
 
+        // Do multi-tag pose estimation
+        MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
+        if (settings.solvePNPEnabled && settings.doMultiTarget) {
+            var multiTagOutput = multiTagPNPPipe.run(targetList);
+            sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
+            multiTagResult = multiTagOutput.output;
+        }
+
+        // Do single-tag pose estimation
+        if (settings.solvePNPEnabled) {
+            // Clear target list that was used for multitag so we can add target transforms
+            targetList.clear();
+            // TODO global state again ew
+            var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
+
+            for (ArucoDetectionResult detection : tagDetectionPipeResult.output) {
+                AprilTagPoseEstimate tagPoseEstimate = null;
+                // Do single-tag estimation when "always enabled" or if a tag was not used for multitag
+                if (settings.doSingleTargetAlways
+                        || !multiTagResult.fiducialIDsUsed.contains(detection.getId())) {
+                    var poseResult = singleTagPoseEstimatorPipe.run(detection);
+                    sumPipeNanosElapsed += poseResult.nanosElapsed;
+                    tagPoseEstimate = poseResult.output;
+                }
+
+                // If single-tag estimation was not done, this is a multi-target tag from the layout
+                if (tagPoseEstimate == null) {
+                    // compute this tag's camera-to-tag transform using the multitag result
+                    var tagPose = atfl.getTagPose(detection.getId());
+                    if (tagPose.isPresent()) {
+                        var camToTag =
+                                new Transform3d(
+                                        new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
+                        // match expected OpenCV coordinate system
+                        camToTag =
+                                CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());
+
+                        tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0);
+                    }
+                }
+
+                // populate the target list
+                // Challenge here is that TrackedTarget functions with OpenCV Contour
+                TrackedTarget target =
+                        new TrackedTarget(
+                                detection,
+                                tagPoseEstimate,
+                                new TargetCalculationParameters(
+                                        false, null, null, null, null, frameStaticProperties));
+
+                var correctedBestPose =
+                        MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d());
+                var correctedAltPose =
+                        MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d());
+
+                target.setBestCameraToTarget3d(
+                        new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
+                target.setAltCameraToTarget3d(
+                        new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
+
+                targetList.add(target);
+            }
+        }
+
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
-        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, frame);
+        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
+    }
+
+    private void drawThresholdFrame(Mat greyMat, Mat outputMat, int windowSize, double constant) {
+        if (windowSize % 2 == 0) windowSize++;
+        Imgproc.adaptiveThreshold(
+                greyMat,
+                outputMat,
+                255,
+                Imgproc.ADAPTIVE_THRESH_MEAN_C,
+                Imgproc.THRESH_BINARY_INV,
+                windowSize,
+                constant);
     }
 }

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

@@ -18,23 +18,35 @@
 package org.photonvision.vision.pipeline;
 
 import com.fasterxml.jackson.annotation.JsonTypeName;
+import org.photonvision.common.util.numbers.IntegerCouple;
+import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.target.TargetModel;
 
 @JsonTypeName("ArucoPipelineSettings")
 public class ArucoPipelineSettings extends AdvancedPipelineSettings {
-    public double decimate = 1;
+    public AprilTagFamily tagFamily = AprilTagFamily.kTag16h5;
-    public int threads = 2;
-    public int numIterations = 100;
-    public double cornerAccuracy = 25.0;
-    public boolean useAruco3 = true;
 
-    // 3d settings
+    public IntegerCouple threshWinSizes = new IntegerCouple(11, 91);
+    public int threshStepSize = 40;
+    public double threshConstant = 10;
+    public boolean debugThreshold = false;
+
+    public boolean useCornerRefinement = true;
+    public int refineNumIterations = 30;
+    public double refineMinErrorPx = 0.005;
+
+    public boolean useAruco3 = false;
+    public double aruco3MinMarkerSideRatio = 0.02;
+    public int aruco3MinCanonicalImgSide = 32;
+
+    public boolean doMultiTarget = false;
+    public boolean doSingleTargetAlways = false;
 
     public ArucoPipelineSettings() {
         super();
         pipelineType = PipelineType.Aruco;
         outputShowMultipleTargets = true;
-        targetModel = TargetModel.kAruco6in_16h5;
+        targetModel = TargetModel.k6in_16h5;
         cameraExposure = -1;
         cameraAutoExposure = true;
         ledMode = false;

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

@@ -114,14 +114,8 @@ 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( // Corners of the tag's inner black square (excluding white border)
+    k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
-            List.of(
+            // do not
-                    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( // Corners of the tag's inner black square (excluding white border)
             List.of(
                     new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0),
                     new Point3(-Units.inchesToMeters(3), Units.inchesToMeters(3), 0),

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

@@ -18,10 +18,7 @@ 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.ArrayList;
 import java.util.HashMap;
 import java.util.List;
@@ -147,25 +144,12 @@ public class TrackedTarget implements Releasable {
 
         // 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);
-        MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
-        setCameraRelativeRvec(rvec);
     }
 
-    public TrackedTarget(ArucoDetectionResult result, TargetCalculationParameters params) {
+    public TrackedTarget(
+            ArucoDetectionResult result,
+            AprilTagPoseEstimate tagPose,
+            TargetCalculationParameters params) {
         m_targetOffsetPoint = new Point(result.getCenterX(), result.getCenterY());
         m_robotOffsetPoint = new Point();
         var yawPitch =
@@ -179,8 +163,8 @@ public class TrackedTarget implements Releasable {
         m_yaw = yawPitch.getFirst();
         m_pitch = yawPitch.getSecond();
 
-        double[] xCorners = result.getxCorners();
+        double[] xCorners = result.getXCorners();
-        double[] yCorners = result.getyCorners();
+        double[] yCorners = result.getYCorners();
 
         Point[] cornerPoints =
                 new Point[] {
@@ -198,25 +182,38 @@ public class TrackedTarget implements Releasable {
         m_shape = null;
 
         // TODO implement skew? or just yeet
+        m_skew = 0;
 
-        var tvec = new Mat(3, 1, CvType.CV_64FC1);
+        var bestPose = new Transform3d();
-        tvec.put(0, 0, result.getTvec());
+        var altPose = new Transform3d();
-        setCameraRelativeTvec(tvec);
+        if (tagPose != null) {
+            if (tagPose.error1 <= tagPose.error2) {
+                bestPose = tagPose.pose1;
+                altPose = tagPose.pose2;
+            } else {
+                bestPose = tagPose.pose2;
+                altPose = tagPose.pose1;
+            }
 
-        var rvec = new Mat(3, 1, CvType.CV_64FC1);
+            m_bestCameraToTarget3d = bestPose;
-        rvec.put(0, 0, result.getRvec());
+            m_altCameraToTarget3d = altPose;
-        setCameraRelativeRvec(rvec);
 
-        {
+            m_poseAmbiguity = tagPose.getAmbiguity();
-            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());
 
-            m_bestCameraToTarget3d =
+            var tvec = new Mat(3, 1, CvType.CV_64FC1);
-                    MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation));
+            tvec.put(
+                    0,
+                    0,
+                    new double[] {
+                        bestPose.getTranslation().getX(),
+                        bestPose.getTranslation().getY(),
+                        bestPose.getTranslation().getZ()
+                    });
+            setCameraRelativeTvec(tvec);
+
+            var rvec = new Mat(3, 1, CvType.CV_64FC1);
+            MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
+            setCameraRelativeRvec(rvec);
         }
     }
 
@@ -410,8 +407,8 @@ public class TrackedTarget implements Releasable {
             }
             {
                 var points = t.getTargetCorners();
-                for (int i = 0; i < points.size(); i++) {
+                for (Point point : points) {
-                    detectedCorners.add(new TargetCorner(points.get(i).x, points.get(i).y));
+                    detectedCorners.add(new TargetCorner(point.x, point.y));
                 }
             }
 

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

@@ -22,6 +22,7 @@ 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.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.camera.QuirkyCamera;
@@ -32,8 +33,9 @@ import org.photonvision.vision.target.TrackedTarget;
 
 public class AprilTagTest {
     @BeforeEach
-    public void Init() {
+    public void setup() {
         TestUtils.loadLibraries();
+        ConfigManager.getInstance().load();
     }
 
     @Test
@@ -56,13 +58,8 @@ public class AprilTagTest {
         frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
 
         CVPipelineResult pipelineResult;
-        try {
+        pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
-            pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+        printTestResults(pipelineResult);
-            printTestResults(pipelineResult);
-        } catch (RuntimeException e) {
-            // For now, will throw because of the Rotation3d ctor
-            return;
-        }
 
         // Draw on input
         var outputPipe = new OutputStreamPipeline();
@@ -73,11 +70,26 @@ public class AprilTagTest {
         TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
 
         // these numbers are not *accurate*, but they are known and expected
-        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
+        var target = pipelineResult.targets.get(0);
+
+        // Test corner order
+        var corners = target.getTargetCorners();
+        Assertions.assertEquals(260, corners.get(0).x, 10);
+        Assertions.assertEquals(245, corners.get(0).y, 10);
+        Assertions.assertEquals(315, corners.get(1).x, 10);
+        Assertions.assertEquals(245, corners.get(1).y, 10);
+        Assertions.assertEquals(315, corners.get(2).x, 10);
+        Assertions.assertEquals(190, corners.get(2).y, 10);
+        Assertions.assertEquals(260, corners.get(3).x, 10);
+        Assertions.assertEquals(190, corners.get(3).y, 10);
+
+        var pose = target.getBestCameraToTarget3d();
+        // Test pose estimate translation
         Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
         Assertions.assertEquals(0.1, pose.getTranslation().getY(), 0.2);
         Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2);
 
+        // Test pose estimate rotation
         // We expect the object axes to be in NWU, with the x-axis coming out of the tag
         // This visible tag is facing the camera almost parallel, so in world space:
 
@@ -111,13 +123,8 @@ public class AprilTagTest {
         frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
 
         CVPipelineResult pipelineResult;
-        try {
+        pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
-            pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+        printTestResults(pipelineResult);
-            printTestResults(pipelineResult);
-        } catch (RuntimeException e) {
-            // For now, will throw because of the Rotation3d ctor
-            return;
-        }
 
         // Draw on input
         var outputPipe = new OutputStreamPipeline();

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

@@ -17,11 +17,14 @@
 
 package org.photonvision.vision.pipeline;
 
-import java.io.IOException;
+import edu.wpi.first.math.geometry.Translation3d;
 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.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.TestUtils;
+import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.frame.provider.FileFrameProvider;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
@@ -30,12 +33,13 @@ import org.photonvision.vision.target.TrackedTarget;
 
 public class ArucoPipelineTest {
     @BeforeEach
-    public void Init() throws IOException {
+    public void setup() {
         TestUtils.loadLibraries();
+        ConfigManager.getInstance().load();
     }
 
     @Test
-    public void testApriltagFacingCameraAruco() {
+    public void testApriltagFacingCamera() {
         var pipeline = new ArucoPipeline();
 
         pipeline.getSettings().inputShouldShow = true;
@@ -44,32 +48,59 @@ public class ArucoPipelineTest {
         pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
         pipeline.getSettings().cornerDetectionUseConvexHulls = true;
         pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
-
+        pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
-        // pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
 
         var frameProvider =
                 new FileFrameProvider(
-                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_16h5_1280, false),
+                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_640_480, false),
-                        106,
+                        TestUtils.WPI2020Image.FOV,
-                        TestUtils.getCoeffs("laptop_1280.json", false));
+                        TestUtils.get2020LifeCamCoeffs(false));
         frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
 
         CVPipelineResult pipelineResult;
-        try {
+        pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
-            pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+        printTestResults(pipelineResult);
-            printTestResults(pipelineResult);
-        } catch (RuntimeException e) {
-            // For now, will throw because of the Rotation3d ctor
-            return;
-        }
 
         // Draw on input
         var outputPipe = new OutputStreamPipeline();
-        outputPipe.process(
+        var ret =
-                pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
+                outputPipe.process(
+                        pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
 
-        TestUtils.showImage(
+        TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
-                pipelineResult.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
+
+        // these numbers are not *accurate*, but they are known and expected
+        var target = pipelineResult.targets.get(0);
+
+        // Test corner order
+        var corners = target.getTargetCorners();
+        Assertions.assertEquals(260, corners.get(0).x, 10);
+        Assertions.assertEquals(245, corners.get(0).y, 10);
+        Assertions.assertEquals(315, corners.get(1).x, 10);
+        Assertions.assertEquals(245, corners.get(1).y, 10);
+        Assertions.assertEquals(315, corners.get(2).x, 10);
+        Assertions.assertEquals(190, corners.get(2).y, 10);
+        Assertions.assertEquals(260, corners.get(3).x, 10);
+        Assertions.assertEquals(190, corners.get(3).y, 10);
+
+        var pose = target.getBestCameraToTarget3d();
+        // Test pose estimate translation
+        Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
+        Assertions.assertEquals(0.1, pose.getTranslation().getY(), 0.2);
+        Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2);
+
+        // Test pose estimate rotation
+        // We expect the object axes to be in NWU, with the x-axis coming out of the tag
+        // This visible tag is facing the camera almost parallel, so in world space:
+
+        // The object's X axis should be (-1, 0, 0)
+        Assertions.assertEquals(
+                -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.1);
+        // The object's Y axis should be (0, -1, 0)
+        Assertions.assertEquals(
+                -1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.1);
+        // The object's Z axis should be (0, 0, 1)
+        Assertions.assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.1);
     }
 
     private static void printTestResults(CVPipelineResult pipelineResult) {