Charuco Support (#1312)

Add charuco calibration to photonvision. Currently does not support generating custom charuco boards. This does not support https://calib.io/pages/camera-calibration-pattern-generator. Currently only supports the 4X4_50 family. Also removes all dotboard calibration. Fixes using the lowest possible fps while doing calibration (now uses the highest fps available for each resolution).

build.gradle

@@ -32,7 +32,7 @@ ext {
     libcameraDriverVersion = "dev-v2023.1.0-10-g2693ec0"
     rknnVersion = "dev-v2024.0.0-64-gc0836a6"
     frcYear = "2024"
-    mrcalVersion = "dev-v2024.0.0-18-gb903a09";
+    mrcalVersion = "dev-v2024.0.0-23-g9620baa";
 
 
     pubVersion = versionString

photon-client/src/components/cameras/CameraCalibrationCard.vue

@@ -5,6 +5,7 @@ import { CalibrationBoardTypes, type VideoFormat } from "@/types/SettingTypes";
 import JsPDF from "jspdf";
 import { font as PromptRegular } from "@/assets/fonts/PromptRegular";
 import MonoLogo from "@/assets/images/logoMono.png";
+import CharucoImage from "@/assets/images/ChArUco_Marker8x8.png";
 import PvSlider from "@/components/common/pv-slider.vue";
 import { useStateStore } from "@/stores/StateStore";
 import PvSwitch from "@/components/common/pv-switch.vue";
@@ -19,10 +20,17 @@ const settingsValid = ref(true);
 
 const getUniqueVideoFormatsByResolution = (): VideoFormat[] => {
   const uniqueResolutions: VideoFormat[] = [];
-  useCameraSettingsStore().currentCameraSettings.validVideoFormats.forEach((format, index) => {
-    if (!uniqueResolutions.some((v) => resolutionsAreEqual(v.resolution, format.resolution))) {
-      format.index = index;
+  useCameraSettingsStore().currentCameraSettings.validVideoFormats.forEach((format) => {
+    const index = uniqueResolutions.findIndex((v) => resolutionsAreEqual(v.resolution, format.resolution));
+    const contains = index != -1;
+    let skip = false;
+    if (contains && format.fps > uniqueResolutions[index].fps) {
+      uniqueResolutions.splice(index, 1);
+    } else if (contains) {
+      skip = true;
+    }
 
+    if (!skip) {
       const calib = useCameraSettingsStore().getCalibrationCoeffs(format.resolution);
       if (calib !== undefined) {
         // For each error, square it, sum the squares, and divide by total points N
@@ -53,11 +61,11 @@ const getUniqueVideoFormatsByResolution = (): VideoFormat[] => {
   );
   return uniqueResolutions;
 };
+
 const getUniqueVideoResolutionStrings = (): { name: string; value: number }[] =>
   getUniqueVideoFormatsByResolution().map<{ name: string; value: number }>((f) => ({
     name: `${getResolutionString(f.resolution)}`,
-    // Index won't ever be undefined
-    value: f.index || 0
+    value: f.index || 0 // Index won't ever be undefined
   }));
 const calibrationDivisors = computed(() =>
   [1, 2, 4].filter((v) => {
@@ -67,9 +75,10 @@ const calibrationDivisors = computed(() =>
 );
 
 const squareSizeIn = ref(1);
+const markerSizeIn = ref(0.75);
 const patternWidth = ref(8);
 const patternHeight = ref(8);
-const boardType = ref<CalibrationBoardTypes>(CalibrationBoardTypes.Chessboard);
+const boardType = ref<CalibrationBoardTypes>(CalibrationBoardTypes.Charuco);
 const useMrCalRef = ref(true);
 const useMrCal = computed<boolean>({
   get() {
@@ -109,22 +118,23 @@ const downloadCalibBoard = () => {
           }
         }
       }
+      doc.text(`${patternWidth.value} x ${patternHeight.value} | ${squareSizeIn.value}in`, paperWidth - 1, 1.0, {
+        maxWidth: (paperWidth - 2.0) / 2,
+        align: "right"
+      });
       break;
-    case CalibrationBoardTypes.DotBoard:
-      // eslint-disable-next-line no-case-declarations
-      const dotgridStartX =
-        (paperWidth - (2 * (patternWidth.value - 1) + ((patternHeight.value - 1) % 2)) * squareSizeIn.value) / 2.0;
-      // eslint-disable-next-line no-case-declarations
-      const dotgridStartY = (paperHeight - (patternHeight.value - squareSizeIn.value)) / 2;
 
-      for (let squareY = 0; squareY < patternHeight.value; squareY++) {
-        for (let squareX = 0; squareX < patternWidth.value; squareX++) {
-          const xPos = dotgridStartX + (2 * squareX + (squareY % 2)) * squareSizeIn.value;
-          const yPos = dotgridStartY + squareY * squareSizeIn.value;
+    case CalibrationBoardTypes.Charuco:
+      // Add pregenerated charuco
+      const charucoImage = new Image();
+      charucoImage.src = CharucoImage;
+      doc.addImage(charucoImage, "PNG", 0.25, 1.5, 8, 8);
+
+      doc.text(`8 x 8 | 1in & 0.75in`, paperWidth - 1, 1.0, {
+        maxWidth: (paperWidth - 2.0) / 2,
+        align: "right"
+      });
 
-          doc.circle(xPos, yPos, squareSizeIn.value / 4, "F");
-        }
-      }
       break;
   }
 
@@ -146,11 +156,6 @@ const downloadCalibBoard = () => {
   logoImage.src = MonoLogo;
   doc.addImage(logoImage, "PNG", 1.0, 0.75, 1.4, 0.5);
 
-  doc.text(`${patternWidth.value} x ${patternHeight.value} | ${squareSizeIn.value}in`, paperWidth - 1, 1.0, {
-    maxWidth: (paperWidth - 2.0) / 2,
-    align: "right"
-  });
-
   doc.save(`calibrationTarget-${CalibrationBoardTypes[boardType.value]}.pdf`);
 };
 
@@ -191,6 +196,7 @@ const isCalibrating = ref(false);
 const startCalibration = () => {
   useCameraSettingsStore().startPnPCalibration({
     squareSizeIn: squareSizeIn.value,
+    markerSizeIn: markerSizeIn.value,
     patternHeight: patternHeight.value,
     patternWidth: patternWidth.value,
     boardType: boardType.value,
@@ -280,7 +286,7 @@ const setSelectedVideoFormat = (format: VideoFormat) => {
               :items="getUniqueVideoResolutionStrings()"
             />
             <pv-select
-              v-show="isCalibrating"
+              v-show="isCalibrating && boardType != CalibrationBoardTypes.Charuco"
               v-model="useCameraSettingsStore().currentPipelineSettings.streamingFrameDivisor"
               label="Decimation"
               tooltip="Resolution to which camera frames are downscaled for detection. Calibration still uses full-res"
@@ -293,7 +299,7 @@ const setSelectedVideoFormat = (format: VideoFormat) => {
               label="Board Type"
               tooltip="Calibration board pattern to use"
               :select-cols="7"
-              :items="['Chessboard', 'Dotboard']"
+              :items="['Chessboard', 'Charuco']"
               :disabled="isCalibrating"
             />
             <pv-number-input
@@ -304,6 +310,15 @@ const setSelectedVideoFormat = (format: VideoFormat) => {
               :rules="[(v) => v > 0 || 'Size must be positive']"
               :label-cols="5"
             />
+            <pv-number-input
+              v-model="markerSizeIn"
+              v-show="boardType == CalibrationBoardTypes.Charuco"
+              label="Marker Size (in)"
+              tooltip="Size of the tag markers in inches must be smaller than pattern spacing"
+              :disabled="isCalibrating"
+              :rules="[(v) => v > 0 || 'Size must be positive']"
+              :label-cols="5"
+            />
             <pv-number-input
               v-model="patternWidth"
               label="Board Width (squares)"

photon-client/src/stores/settings/CameraSettingsStore.ts

@@ -314,6 +314,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
     startPnPCalibration(
       calibrationInitData: {
         squareSizeIn: number;
+        markerSizeIn: number;
         patternWidth: number;
         patternHeight: number;
         boardType: CalibrationBoardTypes;

photon-client/src/types/SettingTypes.ts

@@ -292,7 +292,7 @@ export const PlaceholderCameraSettings: CameraSettings = {
 
 export enum CalibrationBoardTypes {
   Chessboard = 0,
-  DotBoard = 1
+  Charuco = 1
 }
 
 export enum RobotOffsetType {

photon-client/src/types/WebsocketDataTypes.ts

@@ -77,6 +77,7 @@ export interface WebsocketCalibrationData {
   videoModeIndex: number;
   patternHeight: number;
   squareSizeIn: number;
+  markerSizeIn: number;
 }
 
 export interface IncomingWebsocketData {

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

@@ -340,14 +340,14 @@ public class TestUtils {
         return getPowercellPath(testMode).resolve(image.path);
     }
 
-    public static Path getDotBoardImagesPath() {
-        return getResourcesFolderPath(false).resolve("calibrationBoardImages");
-    }
-
     public static Path getSquaresBoardImagesPath() {
         return getResourcesFolderPath(false).resolve("calibrationSquaresImg");
     }
 
+    public static Path getCharucoBoardImagesPath() {
+        return getResourcesFolderPath(false).resolve("calibrationCharucoImg");
+    }
+
     public static File getHardwareConfigJson() {
         return getResourcesFolderPath(false)
                 .resolve("hardware")

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

@@ -23,10 +23,6 @@ import java.util.List;
 import java.util.stream.Collectors;
 import org.opencv.calib3d.Calib3d;
 import org.opencv.core.*;
-import org.opencv.core.Mat;
-import org.opencv.core.MatOfDouble;
-import org.opencv.core.MatOfPoint2f;
-import org.opencv.core.Size;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.math.MathUtils;
@@ -65,7 +61,8 @@ public class Calibrate3dPipe
     private final Mat stdDeviationsIntrinsics = new Mat();
     private final Mat stdDeviationsExtrinsics = new Mat();
 
-    // Contains the re projection error of each snapshot by re projecting the corners we found and
+    // Contains the re projection error of each snapshot by re projecting the
+    // corners we found and
     // finding the Euclidean distance between the actual corners.
     private final Mat perViewErrors = new Mat();
 
@@ -120,13 +117,31 @@ public class Calibrate3dPipe
 
     protected CameraCalibrationCoefficients calibrateOpenCV(
             List<FindBoardCornersPipe.FindBoardCornersPipeResult> in, double fxGuess, double fyGuess) {
-        List<Mat> objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
-        List<Mat> imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
-        if (objPoints.size() != imgPts.size()) {
+        List<MatOfPoint3f> objPointsIn =
+                in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
+        List<MatOfPoint2f> imgPointsIn =
+                in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
+        List<MatOfFloat> levelsArr = in.stream().map(it -> it.levels).collect(Collectors.toList());
+
+        if (objPointsIn.size() != imgPointsIn.size() || objPointsIn.size() != levelsArr.size()) {
             logger.error("objpts.size != imgpts.size");
             return null;
         }
 
+        // And delete rows depending on the level -- otherwise, level has no impact for opencv
+        List<Mat> objPoints = new ArrayList<>();
+        List<Mat> imgPoints = new ArrayList<>();
+        for (int i = 0; i < objPointsIn.size(); i++) {
+            MatOfPoint3f objPtsOut = new MatOfPoint3f();
+            MatOfPoint2f imgPtsOut = new MatOfPoint2f();
+
+            deleteIgnoredPoints(
+                    objPointsIn.get(i), imgPointsIn.get(i), levelsArr.get(i), objPtsOut, imgPtsOut);
+
+            objPoints.add(objPtsOut);
+            imgPoints.add(imgPtsOut);
+        }
+
         Mat cameraMatrix = new Mat(3, 3, CvType.CV_64F);
         MatOfDouble distortionCoefficients = new MatOfDouble();
         List<Mat> rvecs = new ArrayList<>();
@@ -138,12 +153,13 @@ public class Calibrate3dPipe
         cameraMatrix.put(0, 0, new double[] {fxGuess, 0, cx, 0, fyGuess, cy, 0, 0, 1});
 
         try {
-            // FindBoardCorners pipe outputs all the image points, object points, and frames to calculate
+            // FindBoardCorners pipe outputs all the image points, object points, and frames
+            // to calculate
             // imageSize from, other parameters are output Mats
 
             Calib3d.calibrateCameraExtended(
                     objPoints,
-                    imgPts,
+                    imgPoints,
                     new Size(in.get(0).size.width, in.get(0).size.height),
                     cameraMatrix,
                     distortionCoefficients,
@@ -169,6 +185,8 @@ public class Calibrate3dPipe
         distortionCoefficients.release();
         rvecs.forEach(Mat::release);
         tvecs.forEach(Mat::release);
+        objPoints.forEach(Mat::release);
+        imgPoints.forEach(Mat::release);
 
         return new CameraCalibrationCoefficients(
                 in.get(0).size,
@@ -186,12 +204,16 @@ public class Calibrate3dPipe
         List<MatOfPoint2f> corner_locations =
                 in.stream().map(it -> it.imagePoints).map(MatOfPoint2f::new).collect(Collectors.toList());
 
+        List<MatOfFloat> levels =
+                in.stream().map(it -> it.levels).map(MatOfFloat::new).collect(Collectors.toList());
+
         int imageWidth = (int) in.get(0).size.width;
         int imageHeight = (int) in.get(0).size.height;
 
         MrCalResult result =
                 MrCalJNI.calibrateCamera(
                         corner_locations,
+                        levels,
                         params.boardWidth,
                         params.boardHeight,
                         params.squareSize,
@@ -199,6 +221,9 @@ public class Calibrate3dPipe
                         imageHeight,
                         (fxGuess + fyGuess) / 2.0);
 
+        levels.forEach(MatOfFloat::release);
+        corner_locations.forEach(MatOfPoint2f::release);
+
         // intrinsics are fx fy cx cy from mrcal
         JsonMatOfDouble cameraMatrixMat =
                 new JsonMatOfDouble(
@@ -222,13 +247,38 @@ public class Calibrate3dPipe
         JsonMatOfDouble distortionCoefficientsMat =
                 new JsonMatOfDouble(1, 8, CvType.CV_64FC1, Arrays.copyOfRange(result.intrinsics, 4, 12));
 
-        // Calculate optimized board poses manually. We get this for free from mrcal too, but that's not
-        // JNIed (yet)
+        // Calculate optimized board poses manually. We get this for free from mrcal
+        // too, but that's not JNIed (yet)
         List<Mat> rvecs = new ArrayList<>();
         List<Mat> tvecs = new ArrayList<>();
+
         for (var o : in) {
             var rvec = new Mat();
             var tvec = new Mat();
+
+            // If the calibration points contain points that are negative then we need to exclude them,
+            // they are considered points that we dont want to use in calibration/solvepnp. These points
+            // are required prior to this to allow mrcal to work.
+            Point3[] oPoints = o.objectPoints.toArray();
+            Point[] iPoints = o.imagePoints.toArray();
+
+            List<Point3> outputOPoints = new ArrayList<Point3>();
+            List<Point> outputIPoints = new ArrayList<Point>();
+
+            for (int i = 0; i < iPoints.length; i++) {
+                if (iPoints[i].x >= 0 && iPoints[i].y >= 0) {
+                    outputIPoints.add(iPoints[i]);
+                }
+            }
+            for (int i = 0; i < oPoints.length; i++) {
+                if (oPoints[i].x >= 0 && oPoints[i].y >= 0 && oPoints[i].z >= 0) {
+                    outputOPoints.add(oPoints[i]);
+                }
+            }
+
+            o.objectPoints.fromList(outputOPoints);
+            o.imagePoints.fromList(outputIPoints);
+
             Calib3d.solvePnP(
                     o.objectPoints,
                     o.imagePoints,
@@ -285,7 +335,8 @@ public class Calibrate3dPipe
             // Apply warp, if set
             if (calobject_warp != null && calobject_warp.length == 2) {
                 // mrcal warp model!
-                // The chessboard spans [-1, 1] on the x and y axies. We then let z=k_x(1-x^2)+k_y(1-y^2)
+                // The chessboard spans [-1, 1] on the x and y axies. We then let
+                // z=k_x(1-x^2)+k_y(1-y^2)
 
                 double xmin = 0;
                 double ymin = 0;
@@ -346,6 +397,32 @@ public class Calibrate3dPipe
         return observations;
     }
 
+    /** Delete all rows of mats where level is < 0. Useful for opencv */
+    private void deleteIgnoredPoints(
+            MatOfPoint3f objPtsMatIn,
+            MatOfPoint2f imgPtsMatIn,
+            MatOfFloat levelsMat,
+            MatOfPoint3f objPtsMatOut,
+            MatOfPoint2f imgPtsMatOut) {
+        var levels = levelsMat.toArray();
+        var objPtsIn = objPtsMatIn.toArray();
+        var imgPtsIn = imgPtsMatIn.toArray();
+
+        var objPtsOut = new ArrayList<Point3>();
+        var imgPtsOut = new ArrayList<Point>();
+
+        for (int i = 0; i < levels.length; i++) {
+            if (levels[i] >= 0) {
+                // point survives
+                objPtsOut.add(objPtsIn[i]);
+                imgPtsOut.add(imgPtsIn[i]);
+            }
+        }
+
+        objPtsMatOut.fromList(objPtsOut);
+        imgPtsMatOut.fromList(imgPtsOut);
+    }
+
     public static class CalibratePipeParams {
         // Size (in # of corners) of the calibration object
         public int boardHeight;

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

@@ -17,10 +17,16 @@
 
 package org.photonvision.vision.pipe.impl;
 
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.calib3d.Calib3d;
 import org.opencv.core.*;
 import org.opencv.imgproc.Imgproc;
+import org.opencv.objdetect.CharucoBoard;
+import org.opencv.objdetect.CharucoDetector;
+import org.opencv.objdetect.Objdetect;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.vision.frame.FrameDivisor;
@@ -41,8 +47,12 @@ public class FindBoardCornersPipe
     Size imageSize;
     Size patternSize;
 
+    CharucoBoard board;
+    CharucoDetector detector;
+
     // Configure the optimizations used while using OpenCV's find corners algorithm
-    // Since we return results in real-time, we want to ensure it goes as fast as possible
+    // Since we return results in real-time, we want to ensure it goes as fast as
+    // possible
     // and fails as fast as possible.
     final int findChessboardFlags =
             Calib3d.CALIB_CB_NORMALIZE_IMAGE
@@ -70,18 +80,23 @@ public class FindBoardCornersPipe
         this.objectPoints = null;
         this.objectPoints = new MatOfPoint3f();
 
-        /*If using a chessboard, then the pattern size if the inner corners of the board. For example, the pattern size of a 9x9 chessboard would be 8x8
-        If using a dot board, then the pattern size width is the sum of the bottom 2 rows and the height is the left or right most column
-        For example, a 5x4 dot board would have a pattern size of 11x4
-        We subtract 1 for chessboard because the UI prompts users for the number of squares, not the
-        number of corners.
-        * */
+        /*
+         * If using a chessboard, then the pattern size if the inner corners of the
+         * board. For example, the pattern size of a 9x9 chessboard would be 8x8
+         * If using a dot board, then the pattern size width is the sum of the bottom 2
+         * rows and the height is the left or right most column
+         * For example, a 5x4 dot board would have a pattern size of 11x4
+         * We subtract 1 for chessboard because the UI prompts users for the number of
+         * squares, not the
+         * number of corners.
+         */
         this.patternSize =
                 params.type == UICalibrationData.BoardType.CHESSBOARD
                         ? new Size(params.boardWidth - 1, params.boardHeight - 1)
                         : new Size(params.boardWidth, params.boardHeight);
 
-        // Chessboard and dot board have different 3D points to project as a dot board has alternating
+        // Chessboard and dot board have different 3D points to project as a dot board
+        // has alternating
         // dots per column
         if (params.type == UICalibrationData.BoardType.CHESSBOARD) {
             // Here we can create an NxN grid since a chessboard is rectangular
@@ -92,16 +107,14 @@ public class FindBoardCornersPipe
                     objectPoints.push_back(new MatOfPoint3f(new Point3(boardXCoord, boardYCoord, 0.0)));
                 }
             }
-        } else if (params.type == UICalibrationData.BoardType.DOTBOARD) {
-            // Here we need to alternate the amount of dots per column since a dot board is not
-            // rectangular and also by taking in account the grid size which should be in mm
-            for (int i = 0; i < patternSize.height; i++) {
-                for (int j = 0; j < patternSize.width; j++) {
-                    objectPoints.push_back(
-                            new MatOfPoint3f(
-                                    new Point3((2 * j + i % 2) * params.gridSize, i * params.gridSize, 0.0d)));
-                }
-            }
+        } else if (params.type == UICalibrationData.BoardType.CHARUCOBOARD) {
+            board =
+                    new CharucoBoard(
+                            new Size(params.boardWidth, params.boardHeight),
+                            (float) params.gridSize,
+                            (float) params.markerSize,
+                            Objdetect.getPredefinedDictionary(params.tagFamily));
+            detector = new CharucoDetector(board);
         } else {
             logger.error("Can't create pattern for unknown board type " + params.type);
         }
@@ -219,6 +232,12 @@ public class FindBoardCornersPipe
     private FindBoardCornersPipeResult findBoardCorners(Pair<Mat, Mat> in) {
         createObjectPoints();
 
+        float[] levels = null;
+        var outLevels = new MatOfFloat();
+
+        var objPts = new MatOfPoint3f();
+        var outBoardCorners = new MatOfPoint2f();
+
         var inFrame = in.getLeft();
         var outFrame = in.getRight();
 
@@ -226,9 +245,81 @@ public class FindBoardCornersPipe
         Imgproc.cvtColor(inFrame, inFrame, Imgproc.COLOR_BGR2GRAY);
         boolean boardFound = false;
 
-        if (params.type == UICalibrationData.BoardType.CHESSBOARD) {
+        // Get the size of the inFrame
+        this.imageSize = new Size(inFrame.width(), inFrame.height());
+
+        if (params.type == UICalibrationData.BoardType.CHARUCOBOARD) {
+            Mat objPoints =
+                    new Mat(); // 3 dimensional currentObjectPoints, the physical target ChArUco Board
+            Mat imgPoints =
+                    new Mat(); // 2 dimensional currentImagePoints, the likely distorted board on the flat
+            // camera sensor frame posed relative to the target
+            Mat detectedCorners = new Mat(); // currentCharucoCorners
+            Mat detectedIds = new Mat(); // currentCharucoIds
+            detector.detectBoard(inFrame, detectedCorners, detectedIds);
+
+            // reformat the Mat to a List<Mat> for matchImagePoints
+            final List<Mat> detectedCornersList = new ArrayList<>();
+            for (int i = 0; i < detectedCorners.total(); i++) {
+                detectedCornersList.add(detectedCorners.row(i));
+            }
+
+            if (detectedCornersList.size()
+                    >= 10) { // We need at least 4 corners to be used for calibration but we force 10 just to
+                // ensure the user cant get away with a garbage calibration.
+                boardFound = true;
+            }
+
+            if (!boardFound) {
+                // If we can't find a board, give up
+                return null;
+            }
+            board.matchImagePoints(detectedCornersList, detectedIds, objPoints, imgPoints);
+
+            // draw the charuco board
+            Objdetect.drawDetectedCornersCharuco(
+                    outFrame, detectedCorners, detectedIds, new Scalar(0, 0, 255)); // Red Text
+
+            imgPoints.copyTo(outBoardCorners);
+            objPoints.copyTo(objPts);
+
+            // Since charuco can still detect without the whole board we need to send "fake" (all
+            // values less than zero) points and then tell it to ignore that corner by setting the
+            // corresponding level to -1. Calibrate3dPipe deals with piping this into the correct format
+            // for each backend
+            {
+                Point[] boardCorners =
+                        new Point[(this.params.boardHeight - 1) * (this.params.boardWidth - 1)];
+                Point3[] objectPoints =
+                        new Point3[(this.params.boardHeight - 1) * (this.params.boardWidth - 1)];
+                levels = new float[(this.params.boardHeight - 1) * (this.params.boardWidth - 1)];
+
+                for (int i = 0; i < detectedIds.total(); i++) {
+                    int id = (int) detectedIds.get(i, 0)[0];
+                    boardCorners[id] = outBoardCorners.toList().get(i);
+                    objectPoints[id] = objPts.toList().get(i);
+                    levels[id] = 1.0f;
+                }
+                for (int i = 0; i < boardCorners.length; i++) {
+                    if (boardCorners[i] == null) {
+                        boardCorners[i] = new Point(-1, -1);
+                        objectPoints[i] = new Point3(-1, -1, -1);
+                        levels[i] = -1.0f;
+                    }
+                }
+
+                outBoardCorners.fromArray(boardCorners);
+                outLevels.fromArray(levels);
+            }
+            imgPoints.release();
+            objPoints.release();
+            detectedCorners.release();
+            detectedIds.release();
+
+        } else { // If not Charuco then do chessboard
             // Reduce the image size to be much more manageable
-            // Note that opencv will copy the frame if no resize is requested; we can skip this since we
+            // Note that opencv will copy the frame if no resize is requested; we can skip
+            // this since we
             // don't need that copy. See:
             // https://github.com/opencv/opencv/blob/a8ec6586118c3f8e8f48549a85f2da7a5b78bcc9/modules/imgproc/src/resize.cpp#L4185
             if (params.divisor != FrameDivisor.NONE) {
@@ -242,40 +333,35 @@ public class FindBoardCornersPipe
                     Calib3d.findChessboardCorners(
                             smallerInFrame, patternSize, smallerBoardCorners, findChessboardFlags);
 
-            // Rescale back to original pixel locations
-            if (boardFound) {
-                rescalePointsToOrigFrame(smallerBoardCorners, inFrame, boardCorners);
+            if (!boardFound) {
+                return null;
             }
 
-        } else if (params.type == UICalibrationData.BoardType.DOTBOARD) {
-            boardFound =
-                    Calib3d.findCirclesGrid(
-                            inFrame, patternSize, boardCorners, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
-        }
+            rescalePointsToOrigFrame(smallerBoardCorners, inFrame, boardCorners);
 
+            boardCorners.copyTo(outBoardCorners);
+
+            objectPoints.copyTo(objPts);
+
+            // Do sub corner pix for drawing chessboard when using OpenCV
+            Imgproc.cornerSubPix(
+                    inFrame, outBoardCorners, getWindowSize(outBoardCorners), zeroZone, criteria);
+
+            // draw the chessboard, doesn't have to be different for a dot board since it
+            // just re projects
+            // the corners we found
+            Calib3d.drawChessboardCorners(outFrame, patternSize, outBoardCorners, true);
+
+            levels = new float[(int) objPts.total()];
+            Arrays.fill(levels, 1.0f);
+            outLevels.fromArray(levels);
+        }
         if (!boardFound) {
             // If we can't find a chessboard/dot board, give up
             return null;
         }
 
-        var outBoardCorners = new MatOfPoint2f();
-        boardCorners.copyTo(outBoardCorners);
-
-        var objPts = new MatOfPoint3f();
-        objectPoints.copyTo(objPts);
-
-        // Get the size of the inFrame
-        this.imageSize = new Size(inFrame.width(), inFrame.height());
-
-        // Do sub corner pix for drawing chessboard when using OpenCV
-        Imgproc.cornerSubPix(
-                inFrame, outBoardCorners, getWindowSize(outBoardCorners), zeroZone, criteria);
-
-        // draw the chessboard, doesn't have to be different for a dot board since it just re projects
-        // the corners we found
-        Calib3d.drawChessboardCorners(outFrame, patternSize, outBoardCorners, true);
-
-        return new FindBoardCornersPipeResult(inFrame.size(), objPts, outBoardCorners);
+        return new FindBoardCornersPipeResult(inFrame.size(), objPts, outBoardCorners, outLevels);
     }
 
     public static class FindCornersPipeParams {
@@ -283,18 +369,24 @@ public class FindBoardCornersPipe
         final int boardWidth;
         final UICalibrationData.BoardType type;
         final double gridSize;
+        final double markerSize;
         final FrameDivisor divisor;
+        final int tagFamily;
 
         public FindCornersPipeParams(
                 int boardHeight,
                 int boardWidth,
                 UICalibrationData.BoardType type,
+                int tagFamily,
                 double gridSize,
+                double markerSize,
                 FrameDivisor divisor) {
             this.boardHeight = boardHeight;
             this.boardWidth = boardWidth;
+            this.tagFamily = tagFamily;
             this.type = type;
-            this.gridSize = gridSize; // mm
+            this.gridSize = gridSize; // meter
+            this.markerSize = markerSize; // meter
             this.divisor = divisor;
         }
 
@@ -331,21 +423,24 @@ public class FindBoardCornersPipe
         public Size size;
         public MatOfPoint3f objectPoints;
         public MatOfPoint2f imagePoints;
+        public MatOfFloat levels;
 
         // Set later only if we need it
         public Mat inputImage = null;
 
         public FindBoardCornersPipeResult(
-                Size size, MatOfPoint3f objectPoints, MatOfPoint2f imagePoints) {
+                Size size, MatOfPoint3f objectPoints, MatOfPoint2f imagePoints, MatOfFloat levels) {
             this.size = size;
             this.objectPoints = objectPoints;
             this.imagePoints = imagePoints;
+            this.levels = levels;
         }
 
         @Override
         public void release() {
             objectPoints.release();
             imagePoints.release();
+            levels.release();
             if (inputImage != null) inputImage.release();
         }
     }

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

@@ -15,7 +15,7 @@
  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  */
 
-package org.photonvision.vision.pipe.impl;
+package org.photonvision.vision.pipeline;
 
 import edu.wpi.first.math.util.Units;
 import java.util.ArrayList;
@@ -36,11 +36,11 @@ import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.ImageRotationMode;
 import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
+import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
+import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
 import org.photonvision.vision.pipe.impl.Calibrate3dPipe.CalibrationInput;
+import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
 import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
-import org.photonvision.vision.pipeline.CVPipeline;
-import org.photonvision.vision.pipeline.Calibration3dPipelineSettings;
-import org.photonvision.vision.pipeline.UICalibrationData;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
 
@@ -87,7 +87,9 @@ public class Calibrate3dPipeline
                         settings.boardHeight,
                         settings.boardWidth,
                         settings.boardType,
+                        settings.tagFamily,
                         settings.gridSize,
+                        settings.markerSize,
                         settings.streamingFrameDivisor);
         findBoardCornersPipe.setParams(findCornersPipeParams);
 
@@ -106,7 +108,8 @@ public class Calibrate3dPipeline
         }
 
         if (getSettings().inputImageRotationMode != ImageRotationMode.DEG_0) {
-            // All this calibration assumes zero rotation. If we want a rotation, it should be applied at
+            // All this calibration assumes zero rotation. If we want a rotation, it should
+            // be applied at
             // the output
             logger.error(
                     "Input image rotation was non-zero! Calibration wasn't designed to deal with this. Attempting to manually change back to zero");
@@ -120,11 +123,10 @@ public class Calibrate3dPipeline
         var outputColorCVMat = new CVMat();
         inputColorMat.copyTo(outputColorCVMat.getMat());
 
-        FindBoardCornersPipeResult findBoardResult =
-                findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorCVMat.getMat())).output;
+        FindBoardCornersPipeResult findBoardResult;
 
-        var fpsResult = calculateFPSPipe.run(null);
-        var fps = fpsResult.output;
+        findBoardResult =
+                findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorCVMat.getMat())).output;
 
         if (takeSnapshot) {
             // Set snapshot to false even if we don't find a board
@@ -141,9 +143,13 @@ public class Calibrate3dPipeline
             }
         }
 
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
         frame.release();
 
-        // Return the drawn chessboard if corners are found, if not, then return the input image.
+        // Return the drawn chessboard if corners are found, if not, then return the
+        // input image.
         return new CalibrationPipelineResult(
                 sumPipeNanosElapsed,
                 fps, // Unused but here in case
@@ -175,8 +181,11 @@ public class Calibrate3dPipeline
 
         this.calibrating = true;
 
-        /*Pass the board corners to the pipe, which will check again to see if all boards are valid
-        and returns the corresponding image and object points*/
+        /*
+         * Pass the board corners to the pipe, which will check again to see if all
+         * boards are valid
+         * and returns the corresponding image and object points
+         */
         calibrationOutput =
                 calibrate3dPipe.run(new CalibrationInput(foundCornersList, frameStaticProperties));
 
@@ -209,6 +218,7 @@ public class Calibrate3dPipeline
                                 minSnapshots,
                                 hasEnough(),
                                 Units.metersToInches(settings.gridSize),
+                                Units.metersToInches(settings.markerSize),
                                 settings.boardWidth,
                                 settings.boardHeight,
                                 settings.boardType,
@@ -234,6 +244,7 @@ public class Calibrate3dPipeline
 
     @Override
     public void release() {
-        // we never actually need to give resources up since pipelinemanager only makes one of us
+        // we never actually need to give resources up since pipelinemanager only makes
+        // one of us
     }
 }

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

@@ -19,13 +19,16 @@ package org.photonvision.vision.pipeline;
 
 import edu.wpi.first.math.util.Units;
 import org.opencv.core.Size;
+import org.opencv.objdetect.Objdetect;
 import org.photonvision.vision.frame.FrameDivisor;
 
 public class Calibration3dPipelineSettings extends AdvancedPipelineSettings {
     public int boardHeight = 8;
     public int boardWidth = 8;
     public UICalibrationData.BoardType boardType = UICalibrationData.BoardType.CHESSBOARD;
+    public int tagFamily = Objdetect.DICT_4X4_50;
     public double gridSize = Units.inchesToMeters(1.0);
+    public double markerSize = Units.inchesToMeters(0.75);
 
     public Size resolution = new Size(640, 480);
     public boolean useMrCal = true;

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

@@ -17,8 +17,6 @@
 
 package org.photonvision.vision.pipeline;
 
-import org.photonvision.vision.pipe.impl.Calibrate3dPipeline;
-
 @SuppressWarnings("rawtypes")
 public enum PipelineType {
     Calib3d(-2, Calibrate3dPipeline.class),

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

@@ -27,6 +27,7 @@ public class UICalibrationData {
     public int patternHeight;
     public BoardType boardType;
     public boolean useMrCal;
+    public double markerSizeIn;
 
     public UICalibrationData() {}
 
@@ -36,6 +37,7 @@ public class UICalibrationData {
             int minCount,
             boolean hasEnough,
             double squareSizeIn,
+            double markerSizeIn,
             int patternWidth,
             int patternHeight,
             BoardType boardType,
@@ -45,6 +47,7 @@ public class UICalibrationData {
         this.videoModeIndex = videoModeIndex;
         this.hasEnough = hasEnough;
         this.squareSizeIn = squareSizeIn;
+        this.markerSizeIn = markerSizeIn;
         this.patternWidth = patternWidth;
         this.patternHeight = patternHeight;
         this.boardType = boardType;
@@ -53,7 +56,7 @@ public class UICalibrationData {
 
     public enum BoardType {
         CHESSBOARD,
-        DOTBOARD
+        CHARUCOBOARD,
     }
 
     @Override
@@ -69,6 +72,8 @@ public class UICalibrationData {
                 + hasEnough
                 + ", squareSizeIn="
                 + squareSizeIn
+                + ", markerSizeIn="
+                + markerSizeIn
                 + ", patternWidth="
                 + patternWidth
                 + ", patternHeight="

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

@@ -27,7 +27,6 @@ import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
-import org.photonvision.vision.pipe.impl.Calibrate3dPipeline;
 import org.photonvision.vision.pipeline.*;
 
 @SuppressWarnings({"rawtypes", "unused"})
@@ -72,7 +71,8 @@ public class PipelineManager {
 
         calibration3dPipeline = new Calibrate3dPipeline(uniqueName);
 
-        // We know that at this stage, VisionRunner hasn't yet started so we're good to do this from
+        // We know that at this stage, VisionRunner hasn't yet started so we're good to
+        // do this from
         // this thread
         this.setIndex(defaultIndex);
         updatePipelineFromRequested();
@@ -243,7 +243,8 @@ public class PipelineManager {
      * recreation after changing pipeline type
      */
     private void recreateUserPipeline() {
-        // Cleanup potential old native resources before swapping over from a user pipeline
+        // Cleanup potential old native resources before swapping over from a user
+        // pipeline
         if (currentUserPipeline != null && !(currentPipelineIndex < 0)) {
             currentUserPipeline.release();
         }
@@ -471,7 +472,8 @@ public class PipelineManager {
 
     public void changePipelineType(int newType) {
         // Find the PipelineType proposed
-        // To do this we look at all the PipelineType entries and look for one with matching
+        // To do this we look at all the PipelineType entries and look for one with
+        // matching
         // base indexes
         PipelineType type =
                 Arrays.stream(PipelineType.values())

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

@@ -348,6 +348,7 @@ public class VisionModule {
                         + " and settings "
                         + data);
         settings.gridSize = Units.inchesToMeters(data.squareSizeIn);
+        settings.markerSize = Units.inchesToMeters(data.markerSizeIn);
         settings.boardHeight = data.patternHeight;
         settings.boardWidth = data.patternWidth;
         settings.boardType = data.boardType;
@@ -517,6 +518,7 @@ public class VisionModule {
         // TODO refactor into helper method
         var temp = new HashMap<Integer, HashMap<String, Object>>();
         var videoModes = visionSource.getSettables().getAllVideoModes();
+
         for (var k : videoModes.keySet()) {
             var internalMap = new HashMap<String, Object>();
 

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

@@ -32,6 +32,7 @@ import org.opencv.calib3d.Calib3d;
 import org.opencv.core.Mat;
 import org.opencv.core.Size;
 import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.objdetect.Objdetect;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
@@ -44,7 +45,7 @@ import org.photonvision.vision.frame.FrameDivisor;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.opencv.CVMat;
-import org.photonvision.vision.pipe.impl.Calibrate3dPipeline;
+import org.photonvision.vision.pipeline.UICalibrationData.BoardType;
 
 public class Calibrate3dPipeTest {
     @BeforeAll
@@ -62,17 +63,38 @@ public class Calibrate3dPipeTest {
     }
 
     enum CalibrationDatasets {
-        LIFECAM_480("lifecam/2024-01-02_lifecam_480", new Size(640, 480), new Size(11, 11)),
-        LIFECAM_1280("lifecam/2024-01-02_lifecam_1280", new Size(1280, 720), new Size(11, 11));
+        SQUARES_LIFECAM_480(
+                "lifecam/2024-01-02_lifecam_480",
+                new Size(640, 480),
+                new Size(11, 11),
+                BoardType.CHESSBOARD),
+        SQUARES_LIFECAM_1280(
+                "lifecam/2024-01-02_lifecam_1280",
+                new Size(1280, 720),
+                new Size(11, 11),
+                BoardType.CHESSBOARD),
+
+        CHARUCO_LIFECAM_480(
+                "lifecam/2024-05-07_lifecam_480",
+                new Size(640, 480),
+                new Size(8, 8),
+                BoardType.CHARUCOBOARD),
+        CHARUCO_LIFECAM_1280(
+                "lifecam/2024-05-07_lifecam_1280",
+                new Size(1280, 720),
+                new Size(8, 8),
+                BoardType.CHARUCOBOARD);
 
         final String path;
         final Size size;
         final Size boardSize;
+        final BoardType boardType;
 
-        private CalibrationDatasets(String path, Size image, Size chessboard) {
+        private CalibrationDatasets(String path, Size image, Size chessboard, BoardType boardType) {
             this.path = path;
             this.size = image;
             this.boardSize = chessboard;
+            this.boardType = boardType;
         }
     }
 
@@ -87,45 +109,64 @@ public class Calibrate3dPipeTest {
             @Enum(CalibrationDatasets.class) CalibrationDatasets dataset,
             @Values(booleans = {true, false}) boolean useMrCal) {
         // Pi3 and V1.3 camera
-        String base = TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString();
-        File dir = Path.of(base, dataset.path).toFile();
-        calibrateSquaresCommon(dataset.size, dir, dataset.boardSize, useMrCal);
+        String squareBase = TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString();
+        String charucoBase = TestUtils.getCharucoBoardImagesPath().toAbsolutePath().toString();
+
+        File squareDir = Path.of(squareBase, dataset.path).toFile();
+        File charucoDir = Path.of(charucoBase, dataset.path).toFile();
+
+        if (dataset.boardType == BoardType.CHESSBOARD)
+            calibrateCommon(dataset.size, squareDir, dataset.boardSize, dataset.boardType, useMrCal);
+        else if (dataset.boardType == BoardType.CHESSBOARD)
+            calibrateCommon(dataset.size, charucoDir, dataset.boardSize, dataset.boardType, useMrCal);
     }
 
-    public static void calibrateSquaresCommon(
-            Size imgRes, File rootFolder, Size boardDim, boolean useMrCal) {
-        calibrateSquaresCommon(
+    public static void calibrateCommon(
+            Size imgRes, File rootFolder, Size boardDim, BoardType boardType, boolean useMrCal) {
+        calibrateCommon(
                 imgRes,
                 rootFolder,
                 boardDim,
                 Units.inchesToMeters(1),
+                Units.inchesToMeters(0.75),
+                boardType,
+                Objdetect.DICT_4X4_50,
                 imgRes.width / 2,
                 imgRes.height / 2,
                 useMrCal);
     }
 
-    public static void calibrateSquaresCommon(
+    public static void calibrateCommon(
             Size imgRes,
             File rootFolder,
             Size boardDim,
+            double markerSize,
+            BoardType boardType,
+            int tagFamily,
             double expectedXCenter,
             double expectedYCenter,
             boolean useMrCal) {
-        calibrateSquaresCommon(
+        calibrateCommon(
                 imgRes,
                 rootFolder,
                 boardDim,
                 Units.inchesToMeters(1),
+                markerSize,
+                boardType,
+                tagFamily,
                 expectedXCenter,
                 expectedYCenter,
                 useMrCal);
     }
 
-    public static void calibrateSquaresCommon(
+    public static void calibrateCommon(
             Size imgRes,
             File rootFolder,
             Size boardDim,
             double boardGridSize_m,
+            double markerSize,
+            BoardType boardType,
+            int tagFamily,
             double expectedXCenter,
             double expectedYCenter,
             boolean useMrCal) {
@@ -135,8 +176,11 @@ public class Calibrate3dPipeTest {
 
         assertTrue(directoryListing.length >= 12);
 
-        Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(10, "test_squares_common");
-        calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.CHESSBOARD;
+        Calibrate3dPipeline calibration3dPipeline =
+                new Calibrate3dPipeline(10, "test_calibration_common");
+        calibration3dPipeline.getSettings().boardType = boardType;
+        calibration3dPipeline.getSettings().markerSize = markerSize;
+        calibration3dPipeline.getSettings().tagFamily = tagFamily;
         calibration3dPipeline.getSettings().resolution = imgRes;
         calibration3dPipeline.getSettings().boardHeight = (int) Math.round(boardDim.height);
         calibration3dPipeline.getSettings().boardWidth = (int) Math.round(boardDim.width);
@@ -155,7 +199,8 @@ public class Calibrate3dPipeTest {
                                 new FrameStaticProperties((int) imgRes.width, (int) imgRes.height, 67, null));
                 var output = calibration3dPipeline.run(frame, QuirkyCamera.DefaultCamera);
 
-                // TestUtils.showImage(output.inputAndOutputFrame.processedImage.getMat(), file.getName(),
+                // TestUtils.showImage(output.inputAndOutputFrame.processedImage.getMat(),
+                // file.getName(),
                 // 1);
                 output.release();
                 frame.release();
@@ -176,7 +221,8 @@ public class Calibrate3dPipeTest {
         assertNotNull(cal);
         assertNotNull(cal.observations);
 
-        // Confirm the calibrated center pixel is fairly close to of the "expected" location at the
+        // Confirm the calibrated center pixel is fairly close to of the "expected"
+        // location at the
         // center of the sensor.
         // For all our data samples so far, this should be true.
         double centerXErrPct =
@@ -190,7 +236,8 @@ public class Calibrate3dPipeTest {
         System.out.println("Dist Coeffs: " + cal.distCoeffs.toString());
 
         // Confirm we didn't get leaky on our mat usage
-        // assertEquals(startMatCount, CVMat.getMatCount()); // TODO Figure out why this doesn't
+        // assertEquals(startMatCount, CVMat.getMatCount()); // TODO Figure out why this
+        // doesn't
         // work in CI
         System.out.println("CVMats left: " + CVMat.getMatCount() + " Start: " + startMatCount);
     }