Add Marker Size, Override unique formats by fps,

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

@@ -17,10 +17,44 @@ import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
 
 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))) {
+    const existingIndex = uniqueResolutions.findIndex((v) => resolutionsAreEqual(v.resolution, format.resolution));
+
+    if (existingIndex !== -1) {
+      const existingFormat = uniqueResolutions[existingIndex];
+      if (format.fps > existingFormat.fps) {
+        format.index = index;
+
+        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[existingIndex] = format;
+      }
+    }
+    else{
       format.index = index;
 
       const calib = useCameraSettingsStore().getCalibrationCoeffs(format.resolution);
@@ -47,6 +81,7 @@ const getUniqueVideoFormatsByResolution = (): VideoFormat[] => {
       }
       uniqueResolutions.push(format);
     }
+
   });
   uniqueResolutions.sort(
     (a, b) => b.resolution.width + b.resolution.height - (a.resolution.width + a.resolution.height)
@@ -67,6 +102,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);
@@ -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,
@@ -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/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/vision/pipe/impl/FindBoardCornersPipe.java

@@ -300,6 +300,7 @@ public static class FindCornersPipeParams {
         final double markerSize;
         final FrameDivisor divisor;
         final int tagFamily;
+        final boolean useMrCal;
 
         public FindCornersPipeParams(
                 int boardHeight,
@@ -308,14 +309,16 @@ public FindCornersPipeParams(
                 int tagFamily,
                 double gridSize,
                 double markerSize,
-                FrameDivisor divisor) {
+                FrameDivisor divisor,
+                boolean useMrCal) {
             this.boardHeight = boardHeight;
             this.boardWidth = boardWidth;
             this.tagFamily = tagFamily;
             this.type = type;
-            this.gridSize = gridSize; // mm
-            this.markerSize = markerSize; // mm
+            this.gridSize = gridSize; // meter
+            this.markerSize = markerSize; // meter
             this.divisor = divisor;
+            this.useMrCal = useMrCal;
         }
 
         @Override
@@ -351,12 +354,11 @@ public static class FindBoardCornersPipeResult implements Releasable {
         public Size size;
         public MatOfPoint3f objectPoints;
         public MatOfPoint2f imagePoints;
+        public MatOfFloat levels;
 
         // Set later only if we need it
         public Mat inputImage = null;
 
-        public MatOfFloat levels = null;
-
         public FindBoardCornersPipeResult(
                 Size size, MatOfPoint3f objectPoints, MatOfPoint2f imagePoints, MatOfFloat levels) {
             this.size = size;
@@ -369,8 +371,8 @@ public FindBoardCornersPipeResult(
         public void release() {
             objectPoints.release();
             imagePoints.release();
+            levels.release();
             if (inputImage != null) inputImage.release();
-            if (levels != null) levels.release();
         }
     }
 }

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

@@ -143,27 +143,30 @@ private FindBoardCornersPipe.FindBoardCornersPipeResult findBoardCorners(Pair<Ma
 
         var outLevels = new MatOfFloat();
 
-        Point[] boardCorners = new Point[(this.params.boardHeight - 1) * (this.params.boardWidth - 1)];
-        Point3[] objectPoints =
-                new Point3[(this.params.boardHeight - 1) * (this.params.boardWidth - 1)];
-        float[] 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[i] = 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;
+        if (params.useMrCal) {
+            Point[] boardCorners =
+                    new Point[(this.params.boardHeight - 1) * (this.params.boardWidth - 1)];
+            Point3[] objectPoints =
+                    new Point3[(this.params.boardHeight - 1) * (this.params.boardWidth - 1)];
+            float[] 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[i] = 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);
+            outBoardCorners.fromArray(boardCorners);
+            outLevels.fromArray(levels);
+        }
 
         // Get the size of the inFrame
         this.imageSize = new Size(inFrame.width(), inFrame.height());

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

@@ -93,7 +93,8 @@ protected void setPipeParamsImpl() {
                         settings.tagFamily,
                         settings.gridSize,
                         settings.markerSize,
-                        settings.streamingFrameDivisor);
+                        settings.streamingFrameDivisor,
+                        settings.useMrCal);
         findBoardCornersPipe.setParams(findCornersPipeParams);
 
         Calibrate3dPipe.CalibratePipeParams calibratePipeParams =
@@ -227,9 +228,9 @@ private void broadcastState() {
                                 minSnapshots,
                                 hasEnough(),
                                 Units.metersToInches(settings.gridSize),
+                                Units.metersToInches(settings.markerSize),
                                 settings.boardWidth,
                                 settings.boardHeight,
-                                settings.markerSize,
                                 settings.boardType,
                                 settings.useMrCal));
 

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

@@ -27,7 +27,7 @@ public class UICalibrationData {
     public int patternHeight;
     public BoardType boardType;
     public boolean useMrCal;
-    public double markerLengthIn;
+    public double markerSizeIn;
 
     public UICalibrationData() {}
 
@@ -37,19 +37,19 @@ public UICalibrationData(
             int minCount,
             boolean hasEnough,
             double squareSizeIn,
+            double markerSizeIn,
             int patternWidth,
             int patternHeight,
-            double markerLengthIn,
             BoardType boardType,
             boolean useMrCal) {
         this.count = count;
         this.minCount = minCount;
         this.videoModeIndex = videoModeIndex;
         this.hasEnough = hasEnough;
         this.squareSizeIn = squareSizeIn;
+        this.markerSizeIn = markerSizeIn;
         this.patternWidth = patternWidth;
         this.patternHeight = patternHeight;
-        this.markerLengthIn = markerLengthIn;
         this.boardType = boardType;
         this.useMrCal = useMrCal;
     }
@@ -73,6 +73,8 @@ public String toString() {
                 + hasEnough
                 + ", squareSizeIn="
                 + squareSizeIn
+                + ", markerSizeIn="
+                + markerSizeIn
                 + ", patternWidth="
                 + patternWidth
                 + ", patternHeight="

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

@@ -348,6 +348,7 @@ public void startCalibration(UICalibrationData data) {
                         + " 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 PhotonConfiguration.UICameraConfiguration toUICameraConfig() {
         // 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>();