Charuco Support

build.gradle

@@ -32,7 +32,7 @@ ext {
     photonGlDriverLibVersion = "dev-v2023.1.0-9-g75fc678"
     rknnVersion = "dev-v2024.0.0-64-gc0836a6"
     frcYear = "2024"
-    mrcalVersion = "dev-v2024.0.0-18-gb903a09";
+    mrcalVersion = "dev-v2024.0.0-22-g23b7f8d";
 
 
     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,42 @@ const getUniqueVideoFormatsByResolution = (): VideoFormat[] => {
   );
   return uniqueResolutions;
 };
+
+// const getUniqueVideoFormatsByResolution = (): VideoFormat[] => {
+//   const uniqueResolutions: VideoFormat[] = [];
+//   useCameraSettingsStore().currentCameraSettings.uniqueVideoFormats.forEach((format) => {
+//     const calib = useCameraSettingsStore().getCalibrationCoeffs(format.resolution);
+//     if (calib !== undefined) {
+//       // For each error, square it, sum the squares, and divide by total points N
+//       if (calib.meanErrors.length) format.mean = calib.meanErrors.reduce((a, b) => a + b, 0) / calib.meanErrors.length;
+//       else format.mean = NaN;
+
+//       format.horizontalFOV =
+//         2 * Math.atan2(format.resolution.width / 2, calib.cameraIntrinsics.data[0]) * (180 / Math.PI);
+//       format.verticalFOV =
+//         2 * Math.atan2(format.resolution.height / 2, calib.cameraIntrinsics.data[4]) * (180 / Math.PI);
+//       format.diagonalFOV =
+//         2 *
+//         Math.atan2(
+//           Math.sqrt(
+//             format.resolution.width ** 2 +
+//               (format.resolution.height / (calib.cameraIntrinsics.data[4] / calib.cameraIntrinsics.data[0])) ** 2
+//           ) / 2,
+//           calib.cameraIntrinsics.data[0]
+//         ) *
+//         (180 / Math.PI);
+//     }
+//     uniqueResolutions.push(format);
+//   });
+//   uniqueResolutions.sort(
+//     (a, b) => b.resolution.width + b.resolution.height - (a.resolution.width + a.resolution.height)
+//   );
+//   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,6 +106,7 @@ 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);
@@ -109,22 +149,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 +187,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 +227,7 @@ const isCalibrating = ref(false);
 const startCalibration = () => {
   useCameraSettingsStore().startPnPCalibration({
     squareSizeIn: squareSizeIn.value,
+    markerSizeIn: markerSizeIn.value,
     patternHeight: patternHeight.value,
     patternWidth: patternWidth.value,
     boardType: boardType.value,
@@ -293,7 +330,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 +341,14 @@ const setSelectedVideoFormat = (format: VideoFormat) => {
               :rules="[(v) => v > 0 || 'Size must be positive']"
               :label-cols="5"
             />
+            <pv-number-input
+              v-model="markerSizeIn"
+              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

@@ -57,6 +57,7 @@ export interface WebsocketCameraSettingsUpdate {
   outputStreamPort: number;
   pipelineNicknames: string[];
   videoFormatList: WebsocketVideoFormat;
+  uniqueFormatList: WebsocketVideoFormat;
   cameraQuirks: QuirkyCamera;
 }
 export interface WebsocketNTUpdate {
@@ -77,6 +78,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

@@ -65,7 +65,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();
 
@@ -138,7 +139,8 @@ 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(
@@ -186,19 +188,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 +231,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 +319,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;