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 static Path getPowercellImagePath(PowercellTestImages image, boolean test
         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.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 CalibrationInput(
     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 @@ protected CameraCalibrationCoefficients process(CalibrationInput in) {
 
     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 @@ protected CameraCalibrationCoefficients calibrateOpenCV(
         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 @@ protected CameraCalibrationCoefficients calibrateOpenCV(
         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,19 +204,26 @@ protected CameraCalibrationCoefficients calibrateMrcal(
         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,
                         imageWidth,
                         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 @@ protected CameraCalibrationCoefficients calibrateMrcal(
         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 @@ private List<BoardObservation> createObservations(
             // 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 @@ private List<BoardObservation> createObservations(
         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;